diff --git a/Jenkinsfile b/Jenkinsfile
index f6d07bc85..d41a1f904 100644
--- a/Jenkinsfile
+++ b/Jenkinsfile
@@ -70,7 +70,7 @@ pipeline {
       }
       steps {
         phone_steps("eon-build", [
-          ["build release2-staging & dashcam-staging", "cd release && PUSH=1 ./build_release2.sh"],
+          ["build release2-staging & dashcam-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
         ])
       }
     }
@@ -87,7 +87,7 @@ pipeline {
       }
       steps {
         phone_steps("tici", [
-          ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release3.sh"],
+          ["build release3-staging & dashcam3-staging", "PUSH=1 $SOURCE_DIR/release/build_release.sh"],
         ])
       }
     }
@@ -170,13 +170,12 @@ pipeline {
                   steps {
                     phone_steps("eon", [
                       ["build", "cd selfdrive/manager && ./build.py"],
-                      ["test athena", "nosetests -s selfdrive/athena/tests/test_athenad_old.py"],
-                      ["test sounds", "nosetests -s selfdrive/ui/tests/test_sounds.py"],
-                      ["test boardd loopback", "nosetests -s selfdrive/boardd/tests/test_boardd_loopback.py"],
+                      ["test sounds", "python selfdrive/ui/tests/test_soundd.py"],
+                      ["test boardd loopback", "python selfdrive/boardd/tests/test_boardd_loopback.py"],
                       ["test loggerd", "python selfdrive/loggerd/tests/test_loggerd.py"],
                       ["test encoder", "python selfdrive/loggerd/tests/test_encoder.py"],
                       ["test logcatd", "python selfdrive/logcatd/tests/test_logcatd_android.py"],
-                      //["test updater", "python installer/updater/test_updater.py"],
+                      ["test updater", "python selfdrive/hardware/eon/test_neos_updater.py"],
                     ])
                   }
                 }
@@ -224,7 +223,7 @@ pipeline {
                   }
                 }
 
-                stage('camerad') {
+                stage('EON camerad') {
                   steps {
                     phone_steps("eon-party", [
                       ["build", "cd selfdrive/manager && ./build.py"],
@@ -234,7 +233,7 @@ pipeline {
                   }
                 }
 
-                stage('Tici camerad') {
+                stage('tici camerad') {
                   steps {
                     phone_steps("tici-party", [
                       ["build", "cd selfdrive/manager && ./build.py"],
diff --git a/README.md b/README.md
index 74906943b..abe87a72f 100755
--- a/README.md
+++ b/README.md
@@ -4,18 +4,11 @@ Table of Contents
 =======================
 
 * [What is openpilot?](#what-is-openpilot)
-* [Integration with Stock Features](#integration-with-stock-features)
-* [Supported Hardware](#supported-hardware)
-* [Supported Cars](#supported-cars)
-* [Community Maintained Cars and Features](#community-maintained-cars-and-features)
-* [Installation Instructions](#installation-instructions)
-* [Limitations of openpilot ALC and LDW](#limitations-of-openpilot-alc-and-ldw)
-* [Limitations of openpilot ACC and FCW](#limitations-of-openpilot-acc-and-fcw)
-* [Limitations of openpilot DM](#limitations-of-openpilot-dm)
+* [Running in a car](#running-in-a-car)
+* [Running on PC](#running-on-pc)
+* [Community and Contributing](#community-and-contributing)
 * [User Data and comma Account](#user-data-and-comma-account)
 * [Safety and Testing](#safety-and-testing)
-* [Testing on PC](#testing-on-pc)
-* [Community and Contributing](#community-and-contributing)
 * [Directory Structure](#directory-structure)
 * [Licensing](#licensing)
 
@@ -24,7 +17,7 @@ Table of Contents
 What is openpilot?
 ------
 
-[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of supported [car makes, models and model years](#supported-cars). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers.
+[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of [supported car makes, models and model years](docs/CARS.md). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers. See more about [the vehicle integration and limitations here](docs/INTEGRATION.md).
 
 <table>
   <tr>
@@ -41,306 +34,32 @@ What is openpilot?
   </tr>
 </table>
 
-Integration with Stock Features
+
+Running in a car
 ------
 
-In all supported cars:
-* Stock Lane Keep Assist (LKA) and stock ALC are replaced by openpilot ALC, which only functions when openpilot is engaged by the user.
-* Stock LDW is replaced by openpilot LDW.
+To use openpilot in a car, you need four things
+* This software. It's free and available right here.
+* One of [the 140+ supported cars](docs/CARS.md). We support Honda, Toyota, Hyundai, Nissan, Kia, Chrysler, Lexus, Acura, Audi, VW, and more. If your car is not supported, but has adaptive cruise control and lane keeping assist, it's likely able to run openpilot.
+* A supported device to run this software. This can be a [comma two](https://comma.ai/shop/products/two), [comma three](https://comma.ai/shop/products/three), or if you like to experiment, a [Ubuntu computer with webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam).
+* A way to connect to your car. With a comma two or three, you need only a [car harness](https://comma.ai/shop/products/car-harness). With an EON Gold or PC, you also need a [black panda](https://comma.ai/shop/products/panda).
 
-Additionally, on specific supported cars (see ACC column in [supported cars](#supported-cars)):
-* Stock ACC is replaced by openpilot ACC.
-* openpilot FCW operates in addition to stock FCW.
+We have detailed instructions for [how to install the device in a car](https://comma.ai/setup).
 
-openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning.
-
-Supported Hardware
+Running on PC
 ------
 
-At the moment, openpilot supports the EON Gold DevKit, [comma two](https://comma.ai/shop/products/two), and [comma three](https://comma.ai/shop/products/three). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect your device to the car. For experimental purposes, openpilot can also run on an Ubuntu computer with external [webcams](https://github.com/commaai/openpilot/tree/master/tools/webcam).
+All of openpilot's services can run as normal on a PC, even without special hardware or a car. To develop or experiment with openpilot you can run openpilot on recorded or simulated data.
 
-Supported Cars
-------
+With openpilot's tools you can plot logs, replay drives and watch the full-res camera streams. See [the tools README](tools/README.md) for more information.
 
-| Make      | Model (US Market Reference)   | Supported Package | ACC              | No ACC accel below | No ALC below      |
-| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
-| Acura     | ILX 2016-19                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 25mph             |
-| Acura     | RDX 2016-18                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Acura     | RDX 2019-21                   | All               | Stock            | 0mph               | 3mph              |
-| Honda     | Accord 2018-20                | All               | Stock            | 0mph               | 3mph              |
-| Honda     | Accord Hybrid 2018-20         | All               | Stock            | 0mph               | 3mph              |
-| Honda     | Civic Hatchback 2017-21       | Honda Sensing     | Stock            | 0mph               | 12mph             |
-| Honda     | Civic Coupe 2016-18           | Honda Sensing     | openpilot        | 0mph               | 12mph             |
-| Honda     | Civic Coupe 2019-20           | All               | Stock            | 0mph               | 2mph<sup>2</sup>  |
-| Honda     | Civic Sedan 2016-18           | Honda Sensing     | openpilot        | 0mph               | 12mph             |
-| Honda     | Civic Sedan 2019-20           | All               | Stock            | 0mph               | 2mph<sup>2</sup>  |
-| Honda     | CR-V 2015-16                  | Touring           | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Honda     | CR-V 2017-21                  | Honda Sensing     | Stock            | 0mph               | 12mph             |
-| Honda     | CR-V Hybrid 2017-2019         | Honda Sensing     | Stock            | 0mph               | 12mph             |
-| Honda     | e 2020                        | All 		          | Stock            | 0mph               | 3mph              |
-| Honda     | Fit 2018-19                   | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Honda     | HR-V 2019-20                  | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Honda     | Insight 2019-21               | All               | Stock            | 0mph               | 3mph              |
-| Honda     | Inspire 2018                  | All               | Stock            | 0mph               | 3mph              |
-| Honda     | Odyssey 2018-20               | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 0mph              |
-| Honda     | Passport 2019                 | All               | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Honda     | Pilot 2016-19                 | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Honda     | Ridgeline 2017-21             | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
-| Hyundai   | Palisade 2020-21              | All               | Stock            | 0mph               | 0mph              |
-| Hyundai   | Sonata 2020-21                | All               | Stock            | 0mph               | 0mph              |
-| Lexus     | CT Hybrid 2017-18             | LSS               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | ES 2019-21                    | All               | openpilot        | 0mph               | 0mph              |
-| Lexus     | ES Hybrid 2017-18             | LSS               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | ES Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
-| Lexus     | IS 2017-2019                  | All               | Stock            | 22mph              | 0mph              |
-| Lexus     | NX 2018                       | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | NX 2020                       | All               | openpilot        | 0mph               | 0mph              |
-| Lexus     | NX Hybrid 2018-19             | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | RX 2016-18                    | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | RX 2020-21                    | All               | openpilot        | 0mph               | 0mph              |
-| Lexus     | RX Hybrid 2016-19             | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Lexus     | RX Hybrid 2020-21             | All               | openpilot        | 0mph               | 0mph              |
-| Lexus     | UX Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Alphard 2020                  | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Avalon 2016-21                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
-| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
-| Toyota    | Camry 2018-20                 | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
-| Toyota    | Camry 2021                    | All               | openpilot        | 0mph<sup>4</sup>   | 0mph              |
-| Toyota    | Camry Hybrid 2018-20          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
-| Toyota    | Camry Hybrid 2021             | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | C-HR 2017-20                  | All               | Stock            | 0mph               | 0mph              |
-| Toyota    | C-HR Hybrid 2017-19           | All               | Stock            | 0mph               | 0mph              |
-| Toyota    | Corolla 2017-19               | All               | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
-| Toyota    | Corolla 2020-22               | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Corolla Hatchback 2019-21     | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Corolla Hybrid 2020-21        | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Highlander 2017-19            | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Highlander 2020-21            | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Highlander Hybrid 2017-19     | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Highlander Hybrid 2020-21     | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Mirai 2021	                  | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Prius 2016-20                 | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Prius 2021                    | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Prius Prime 2017-20           | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Prius Prime 2021              | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Rav4 2016-18                  | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
-| Toyota    | Rav4 2019-21                  | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Rav4 Hybrid 2016-18           | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
-| Toyota    | Rav4 Hybrid 2019-21           | All               | openpilot        | 0mph               | 0mph              |
-| Toyota    | Sienna 2018-20                | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
-
-<sup>1</sup>[Comma Pedal](https://github.com/commaai/openpilot/wiki/comma-pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
-<sup>2</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
-<sup>3</sup>When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
-<sup>4</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
-
-Community Maintained Cars and Features
-------
-
-| Make      | Model (US Market Reference)   | Supported Package | ACC              | No ACC accel below | No ALC below |
-| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
-| Audi      | A3 2014-19                    | Prestige          | Stock            | 0mph               | 0mph         |
-| Audi      | A3 Sportback e-tron 2017-18   | Prestige          | Stock            | 0mph               | 0mph         |
-| Audi      | Q2 2018                       | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Buick     | Regal 2018<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Cadillac  | ATS 2018<sup>1</sup>          | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Chevrolet | Malibu 2017<sup>1</sup>       | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Chevrolet | Volt 2017-18<sup>1</sup>      | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Chrysler  | Pacifica 2017-18              | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
-| Chrysler  | Pacifica 2020                 | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
-| Chrysler  | Pacifica Hybrid 2017-18       | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
-| Chrysler  | Pacifica Hybrid 2019-21       | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
-| Genesis   | G70 2018                      | All               | Stock            | 0mph               | 0mph         |
-| Genesis   | G80 2018                      | All               | Stock            | 0mph               | 0mph         |
-| Genesis   | G90 2018                      | All               | Stock            | 0mph               | 0mph         |
-| GMC       | Acadia 2018<sup>1</sup>       | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Holden    | Astra 2017<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
-| Hyundai   | Elantra 2017-19               | SCC + LKAS        | Stock            | 19mph              | 34mph        |
-| Hyundai   | Elantra 2021                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Elantra Hybrid 2021           | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Genesis 2015-16               | SCC + LKAS        | Stock            | 19mph              | 37mph        |
-| Hyundai   | Ioniq Electric 2019           | SCC + LKAS        | Stock            | 0mph               | 32mph        |
-| Hyundai   | Ioniq Electric 2020           | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Ioniq PHEV 2020               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Kona 2020                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Kona EV 2018-19               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Kona Hybrid 2020              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Santa Fe 2019-20              | All               | Stock            | 0mph               | 0mph         |
-| Hyundai   | Sonata 2018-2019              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Hyundai   | Sonata Hybrid 2021            | All               | Stock            | 0mph               | 0mph         |
-| Hyundai   | Veloster 2019-20              | SCC + LKAS        | Stock            | 5mph               | 0mph         |
-| Jeep      | Grand Cherokee 2016-18        | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
-| Jeep      | Grand Cherokee 2019-20        | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
-| Kia       | Forte 2018-21                 | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Niro EV 2019-21               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Niro Hybrid 2021              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Niro PHEV 2019                | SCC + LKAS        | Stock            | 10mph              | 32mph        |
-| Kia       | Optima 2017                   | SCC + LKAS        | Stock            | 0mph               | 32mph        |
-| Kia       | Optima 2019                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Seltos 2021                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Sorento 2018-19               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Stinger 2018                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Ceed 2019                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Kia       | Telluride 2020                | SCC + LKAS        | Stock            | 0mph               | 0mph         |
-| Nissan    | Altima 2019-20                | ProPILOT          | Stock            | 0mph               | 0mph         |
-| Nissan    | Leaf 2018-20                  | ProPILOT          | Stock            | 0mph               | 0mph         |
-| Nissan    | Rogue 2018-20                 | ProPILOT          | Stock            | 0mph               | 0mph         |
-| Nissan    | X-Trail 2017                  | ProPILOT          | Stock            | 0mph               | 0mph         |
-| SEAT      | Ateca 2018                    | Driver Assistance | Stock            | 0mph               | 0mph         |
-| SEAT      | Leon 2014-2020                | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Škoda     | Kodiaq 2018-19                | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Škoda     | Octavia 2015, 2018-19         | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Škoda     | Octavia RS 2016               | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Škoda     | Scala 2020                    | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Škoda     | Superb 2015-18                | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Subaru    | Ascent 2019                   | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Crosstrek 2018-19             | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Forester 2019-21              | EyeSight          | Stock            | 0mph               | 0mph         |
-| Subaru    | Impreza 2017-19               | EyeSight          | Stock            | 0mph               | 0mph         |
-| Volkswagen| Atlas 2018-19                 | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| e-Golf 2014, 2019-20          | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf 2015-20                  | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf Alltrack 2017-18         | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf GTE 2016                 | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf GTI 2018-20              | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf R 2016-19                | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf SportsVan 2016           | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Golf SportWagen 2015          | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Jetta 2018-20                 | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Jetta GLI 2021                | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Passat 2016-17<sup>2</sup>    | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| T-Cross 2021                  | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Tiguan 2020                   | Driver Assistance | Stock            | 0mph               | 0mph         |
-| Volkswagen| Touran 2017                   | Driver Assistance | Stock            | 0mph               | 0mph         |
-
-<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
-<sup>2</sup>Only includes the MQB Passat sold outside of North America. The NMS Passat made in Chattanooga TN is not yet supported.
-
-Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`.
-
-To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0
-
-Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
-
-Installation Instructions
-------
-
-Install openpilot on a supported device by entering ``https://openpilot.comma.ai`` during the installer setup.
-
-Follow these [video instructions](https://youtu.be/lcjqxCymins) to properly mount the device on the windshield. Note: openpilot features an automatic pose calibration routine and openpilot performance should not be affected by small pitch and yaw misalignments caused by imprecise device mounting.
-
-Before placing the device on your windshield, check the state and local laws and ordinances where you drive. Some state laws prohibit or restrict the placement of objects on the windshield of a motor vehicle.
-
-You will be able to engage openpilot after reviewing the onboarding screens and finishing the calibration procedure.
-
-Limitations of openpilot ALC and LDW
-------
-
-openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
-
-While changing lanes, openpilot is not capable of looking next to you or checking your blind spot. Only nudge the wheel to initiate a lane change after you have confirmed it's safe to do so.
-
-Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
-
-* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
-* The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc.
-* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
-* The device is mounted incorrectly.
-* When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce.
-* In the presence of restricted lanes or construction zones.
-* When driving on highly banked roads or in presence of strong cross-wind.
-* Extremely hot or cold temperatures.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* Driving on hills, narrow, or winding roads.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
-
-Limitations of openpilot ACC and FCW
-------
-
-openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times.
-
-Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to:
-
-* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
-* The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc.
-* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
-* The device is mounted incorrectly.
-* Approaching a toll booth, a bridge or a large metal plate.
-* When driving on roads with pedestrians, cyclists, etc...
-* In presence of traffic signs or stop lights, which are not detected by openpilot at this time.
-* When the posted speed limit is below the user selected set speed. openpilot does not detect speed limits at this time.
-* In presence of vehicles in the same lane that are not moving.
-* When abrupt braking maneuvers are required. openpilot is designed to be limited in the amount of deceleration and acceleration that it can produce.
-* When surrounding vehicles perform close cut-ins from neighbor lanes.
-* Driving on hills, narrow, or winding roads.
-* Extremely hot or cold temperatures.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* Interference from other equipment that generates radar waves.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
-
-Limitations of openpilot DM
-------
-
-openpilot DM should not be considered an exact measurement of the alertness of the driver.
-
-Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to:
-
-* Low light conditions, such as driving at night or in dark tunnels.
-* Bright light (due to oncoming headlights, direct sunlight, etc.).
-* The driver's face is partially or completely outside field of view of the driver facing camera.
-* The driver facing camera is obstructed, covered, or damaged.
-
-The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention.
-
-User Data and comma Account
-------
-
-By default, openpilot uploads the driving data to our servers. You can also access your data by pairing with the comma connect app ([iOS](https://apps.apple.com/us/app/comma-connect/id1456551889), [Android](https://play.google.com/store/apps/details?id=ai.comma.connect&hl=en_US)). We use your data to train better models and improve openpilot for everyone.
-
-openpilot is open source software: the user is free to disable data collection if they wish to do so.
-
-openpilot logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
-The driver facing camera is only logged if you explicitly opt-in in settings. The microphone is not recorded.
-
-By using openpilot, you agree to [our Privacy Policy](https://connect.comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data.
-
-Safety and Testing
-----
-
-* openpilot observes ISO26262 guidelines, see [SAFETY.md](SAFETY.md) for more details.
-* openpilot has software in the loop [tests](.github/workflows/selfdrive_tests.yaml) that run on every commit.
-* The safety model code lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details.
-* panda has software in the loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety).
-* Internally, we have a hardware in the loop Jenkins test suite that builds and unit tests the various processes.
-* panda has additional hardware in the loop [tests](https://github.com/commaai/panda/blob/master/Jenkinsfile).
-* We run the latest openpilot in a testing closet containing 10 comma devices continuously replaying routes.
-
-Testing on PC
-------
-For simplified development and experimentation, openpilot can be run in the CARLA driving simulator, which allows you to develop openpilot without a car. The whole setup should only take a few minutes.
-
-Steps:
-1) Start the CARLA server on first terminal
-```
-bash -c "$(curl https://raw.githubusercontent.com/commaai/openpilot/master/tools/sim/start_carla.sh)"
-```
-2) Start openpilot on second terminal
-```
-bash -c "$(curl https://raw.githubusercontent.com/commaai/openpilot/master/tools/sim/start_openpilot_docker.sh)"
-```
-3) Press 1 to engage openpilot
-
-See the full [README](tools/sim/README.md)
-
-You should also take a look at the tools directory in master: lots of tools you can use to replay driving data, test, and develop openpilot from your PC.
+You can also run openpilot in simulation [with the CARLA simulator](tools/sim/README.md). This allows openpilot to drive around a virtual car on your Ubuntu machine. The whole setup should only take a few minutes, but does require a decent GPU.
 
 
 Community and Contributing
 ------
 
-openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). Bug fixes and new car ports are encouraged.
+openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). Bug fixes and new car ports are encouraged. Check out [the contributing docs](docs/CONTRIBUTING.md).
 
 You can add support for your car by following guides we have written for [Brand](https://blog.comma.ai/how-to-write-a-car-port-for-openpilot/) and [Model](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel.
 
@@ -348,16 +67,39 @@ Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/)
 
 And [follow us on Twitter](https://twitter.com/comma_ai).
 
+User Data and comma Account
+------
+
+By default, openpilot uploads the driving data to our servers. You can also access your data through [comma connect](https://connect.comma.ai/). We use your data to train better models and improve openpilot for everyone.
+
+openpilot is open source software: the user is free to disable data collection if they wish to do so.
+
+openpilot logs the road facing cameras, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs.
+The driver facing camera is only logged if you explicitly opt-in in settings. The microphone is not recorded.
+
+By using openpilot, you agree to [our Privacy Policy](https://comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data.
+
+Safety and Testing
+----
+
+* openpilot observes ISO26262 guidelines, see [SAFETY.md](docs/SAFETY.md) for more details.
+* openpilot has software in the loop [tests](.github/workflows/selfdrive_tests.yaml) that run on every commit.
+* The code enforcing the safety model lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details.
+* panda has software in the loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety).
+* Internally, we have a hardware in the loop Jenkins test suite that builds and unit tests the various processes.
+* panda has additional hardware in the loop [tests](https://github.com/commaai/panda/blob/master/Jenkinsfile).
+* We run the latest openpilot in a testing closet containing 10 comma devices continuously replaying routes.
+
 Directory Structure
 ------
     .
     ├── cereal              # The messaging spec and libs used for all logs
     ├── common              # Library like functionality we've developed here
-    ├── installer/updater   # Manages updates of NEOS
+    ├── docs                # Documentation
     ├── opendbc             # Files showing how to interpret data from cars
     ├── panda               # Code used to communicate on CAN
-    ├── phonelibs           # External libraries
-    ├── pyextra             # Extra python packages not shipped in NEOS
+    ├── third_party         # External libraries
+    ├── pyextra             # Extra python packages
     └── selfdrive           # Code needed to drive the car
         ├── assets          # Fonts, images, and sounds for UI
         ├── athena          # Allows communication with the app
@@ -381,7 +123,7 @@ Licensing
 
 openpilot is released under the MIT license. Some parts of the software are released under other licenses as specified.
 
-Any user of this software shall indemnify and hold harmless comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user.
+Any user of this software shall indemnify and hold harmless Comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user.
 
 **THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT.
 YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS.
diff --git a/RELEASES.md b/RELEASES.md
index ca5fea849..64f0814e2 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,34 @@
+Version 0.8.10 (2021-11-01)
+========================
+ * New driving model
+   * Trained on one million minutes!!!
+   * Fixed lead training making lead predictions significantly more accurate
+   * Fixed several localizer dataset bugs and loss function bugs, overall improved accuracy
+ * New driver monitoring model
+   * Trained on latest data from both comma two and comma three
+   * Increased model field of view by 40% on comma three
+   * Improved model stability on masked users
+   * Improved pose prediction with reworked ground-truth stack
+ * Lateral and longitudinal planning MPCs now in ACADOS
+ * Combined longitudinal MPCs
+   * All longitudinal planning now happens in a single MPC system
+   * Fixed instability in MPC problem to prevent sporadic CPU usage
+ * AGNOS 2: minor stability improvements and builder repo open sourced
+ * tools: new and improved replay thanks to deanlee!
+ * Moved community-supported cars outside of the Community Features toggle
+ * Improved FW fingerprinting reliability for Hyundai/Kia/Genesis
+ * Added prerequisites for longitudinal control on Hyundai/Kia/Genesis and Honda Bosch
+ * Audi S3 2015 support thanks to jyoung8607!
+ * Honda Freed 2020 support thanks to belm0!
+ * Hyundai Ioniq Hybrid 2020-2022 support thanks to sunnyhaibin!
+ * Hyundai Santa Fe 2022 support thanks to sunnyhaibin!
+ * Kia K5 2021 support thanks to sunnyhaibin!
+ * Škoda Kamiq 2021 support thanks to jyoung8607!
+ * Škoda Karoq 2019 support thanks to jyoung8607!
+ * Volkswagen Arteon 2021 support thanks to jyoung8607!
+ * Volkswagen California 2021 support thanks to jyoung8607!
+ * Volkswagen Taos 2022 support thanks to jyoung8607!
+
 Version 0.8.9 (2021-09-14)
 ========================
  * Improved fan control on comma three
diff --git a/SConstruct b/SConstruct
index 02a93eb98..18c8d2f12 100644
--- a/SConstruct
+++ b/SConstruct
@@ -37,10 +37,6 @@ AddOption('--compile_db',
           action='store_true',
           help='build clang compilation database')
 
-AddOption('--mpc-generate',
-          action='store_true',
-          help='regenerates the mpc sources')
-
 AddOption('--snpe',
           action='store_true',
           help='use SNPE on PC')
@@ -64,13 +60,21 @@ if arch == "aarch64" and TICI:
   arch = "larch64"
 
 USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
+USE_FRAME_STREAM = os.getenv("USE_FRAME_STREAM") is not None
 
 lenv = {
   "PATH": os.environ['PATH'],
+  "LD_LIBRARY_PATH": [Dir(f"#third_party/acados/{arch}/lib").abspath],
+  "PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath,
+
+  "ACADOS_SOURCE_DIR": Dir("#third_party/acados/acados").abspath,
+  "TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer",
 }
 
+rpath = lenv["LD_LIBRARY_PATH"].copy()
+
 if arch == "aarch64" or arch == "larch64":
-  lenv["LD_LIBRARY_PATH"] = '/data/data/com.termux/files/usr/lib'
+  lenv["LD_LIBRARY_PATH"] += ['/data/data/com.termux/files/usr/lib']
 
   if arch == "aarch64":
     # android
@@ -78,7 +82,7 @@ if arch == "aarch64" or arch == "larch64":
     lenv["ANDROID_ROOT"] = os.environ['ANDROID_ROOT']
 
   cpppath = [
-    "#phonelibs/opencl/include",
+    "#third_party/opencl/include",
   ]
 
   libpath = [
@@ -86,13 +90,14 @@ if arch == "aarch64" or arch == "larch64":
     "/usr/lib",
     "/system/vendor/lib64",
     "/system/comma/usr/lib",
-    "#phonelibs/nanovg",
+    "#third_party/nanovg",
+    f"#third_party/acados/{arch}/lib",
   ]
 
   if arch == "larch64":
     libpath += [
-      "#phonelibs/snpe/larch64",
-      "#phonelibs/libyuv/larch64/lib",
+      "#third_party/snpe/larch64",
+      "#third_party/libyuv/larch64/lib",
       "/usr/lib/aarch64-linux-gnu"
     ]
     cpppath += [
@@ -100,16 +105,16 @@ if arch == "aarch64" or arch == "larch64":
     ]
     cflags = ["-DQCOM2", "-mcpu=cortex-a57"]
     cxxflags = ["-DQCOM2", "-mcpu=cortex-a57"]
-    rpath = ["/usr/local/lib"]
+    rpath += ["/usr/local/lib"]
   else:
+    rpath = []
     libpath += [
-      "#phonelibs/snpe/aarch64",
-      "#phonelibs/libyuv/lib",
+      "#third_party/snpe/aarch64",
+      "#third_party/libyuv/lib",
       "/system/vendor/lib64"
     ]
     cflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
     cxxflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
-    rpath = []
 else:
   cflags = []
   cxxflags = []
@@ -118,7 +123,7 @@ else:
   if arch == "Darwin":
     yuv_dir = "mac" if real_arch != "arm64" else "mac_arm64"
     libpath = [
-      f"#phonelibs/libyuv/{yuv_dir}/lib",
+      f"#third_party/libyuv/{yuv_dir}/lib",
       "/usr/local/lib",
       "/opt/homebrew/lib",
       "/usr/local/opt/openssl/lib",
@@ -134,24 +139,22 @@ else:
     ]
   else:
     libpath = [
-      "#phonelibs/snpe/x86_64-linux-clang",
-      "#phonelibs/libyuv/x64/lib",
-      "#phonelibs/mapbox-gl-native-qt/x86_64",
+      "#third_party/acados/x86_64/lib",
+      "#third_party/snpe/x86_64-linux-clang",
+      "#third_party/libyuv/x64/lib",
+      "#third_party/mapbox-gl-native-qt/x86_64",
       "#cereal",
       "#selfdrive/common",
       "/usr/lib",
       "/usr/local/lib",
     ]
 
-  rpath = [
-    "phonelibs/snpe/x86_64-linux-clang",
-    "cereal",
-    "selfdrive/common"
+  rpath += [
+    Dir("#third_party/snpe/x86_64-linux-clang").abspath,
+    Dir("#cereal").abspath,
+    Dir("#selfdrive/common").abspath
   ]
 
-  # allows shared libraries to work globally
-  rpath = [os.path.join(os.getcwd(), x) for x in rpath]
-
 if GetOption('asan'):
   ccflags = ["-fsanitize=address", "-fno-omit-frame-pointer"]
   ldflags = ["-fsanitize=address"]
@@ -164,15 +167,12 @@ else:
 
 # no --as-needed on mac linker
 if arch != "Darwin":
-  ldflags += ["-Wl,--as-needed"]
+  ldflags += ["-Wl,--as-needed", "-Wl,--no-undefined"]
 
 # Enable swaglog include in submodules
 cflags += ["-DSWAGLOG"]
 cxxflags += ["-DSWAGLOG"]
 
-# change pythonpath to this
-lenv["PYTHONPATH"] = Dir("#").path
-
 env = Environment(
   ENV=lenv,
   CCFLAGS=[
@@ -191,22 +191,25 @@ env = Environment(
 
   CPPPATH=cpppath + [
     "#",
-    "#phonelibs/catch2/include",
-    "#phonelibs/bzip2",
-    "#phonelibs/libyuv/include",
-    "#phonelibs/openmax/include",
-    "#phonelibs/json11",
-    "#phonelibs/curl/include",
-    "#phonelibs/libgralloc/include",
-    "#phonelibs/android_frameworks_native/include",
-    "#phonelibs/android_hardware_libhardware/include",
-    "#phonelibs/android_system_core/include",
-    "#phonelibs/linux/include",
-    "#phonelibs/snpe/include",
-    "#phonelibs/mapbox-gl-native-qt/include",
-    "#phonelibs/nanovg",
-    "#phonelibs/qrcode",
-    "#phonelibs",
+    "#third_party/acados/include",
+    "#third_party/acados/include/blasfeo/include",
+    "#third_party/acados/include/hpipm/include",
+    "#third_party/catch2/include",
+    "#third_party/bzip2",
+    "#third_party/libyuv/include",
+    "#third_party/openmax/include",
+    "#third_party/json11",
+    "#third_party/curl/include",
+    "#third_party/libgralloc/include",
+    "#third_party/android_frameworks_native/include",
+    "#third_party/android_hardware_libhardware/include",
+    "#third_party/android_system_core/include",
+    "#third_party/linux/include",
+    "#third_party/snpe/include",
+    "#third_party/mapbox-gl-native-qt/include",
+    "#third_party/nanovg",
+    "#third_party/qrcode",
+    "#third_party",
     "#cereal",
     "#opendbc/can",
   ],
@@ -221,7 +224,7 @@ env = Environment(
   CXXFLAGS=["-std=c++1z"] + cxxflags,
   LIBPATH=libpath + [
     "#cereal",
-    "#phonelibs",
+    "#third_party",
     "#opendbc/can",
     "#selfdrive/boardd",
     "#selfdrive/common",
@@ -346,7 +349,7 @@ if GetOption("clazy"):
   qt_env['ENV']['CLAZY_IGNORE_DIRS'] = qt_dirs[0]
   qt_env['ENV']['CLAZY_CHECKS'] = ','.join(checks)
 
-Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM')
+Export('env', 'qt_env', 'arch', 'real_arch', 'SHARED', 'USE_WEBCAM', 'USE_FRAME_STREAM')
 
 SConscript(['selfdrive/common/SConscript'])
 Import('_common', '_gpucommon', '_gpu_libs')
@@ -400,7 +403,7 @@ SConscript(['cereal/SConscript'])
 SConscript(['panda/board/SConscript'])
 SConscript(['opendbc/can/SConscript'])
 
-SConscript(['phonelibs/SConscript'])
+SConscript(['third_party/SConscript'])
 
 SConscript(['common/SConscript'])
 SConscript(['common/kalman/SConscript'])
@@ -410,8 +413,7 @@ SConscript(['selfdrive/camerad/SConscript'])
 SConscript(['selfdrive/modeld/SConscript'])
 
 SConscript(['selfdrive/controls/lib/cluster/SConscript'])
-SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript'])
-SConscript(['selfdrive/controls/lib/lead_mpc_lib/SConscript'])
+SConscript(['selfdrive/controls/lib/lateral_mpc_lib/SConscript'])
 SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript'])
 
 SConscript(['selfdrive/boardd/SConscript'])
diff --git a/cereal/SConscript b/cereal/SConscript
index 10f3ab7f5..df066dc51 100644
--- a/cereal/SConscript
+++ b/cereal/SConscript
@@ -9,16 +9,11 @@ messaging_dir = Dir('messaging')
 # Build cereal
 
 schema_files = ['log.capnp', 'car.capnp', 'legacy.capnp']
-env.Command(["gen/c/include/c++.capnp.h", "gen/c/include/java.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS")
+env.Command(["gen/c/include/c++.capnp.h"], [], "mkdir -p " + gen_dir.path + "/c/include && touch $TARGETS")
 env.Command([f'gen/cpp/{s}.c++' for s in schema_files] + [f'gen/cpp/{s}.h' for s in schema_files],
             schema_files,
             f"capnpc --src-prefix={cereal_dir.path} $SOURCES -o c++:{gen_dir.path}/cpp/")
 
-if shutil.which('capnpc-java'):
-  env.Command(['gen/java/Car.java', 'gen/java/Log.java'],
-              schema_files,
-              f"capnpc $SOURCES --src-prefix={cereal_dir.path} -o java:{gen_dir.path}/java/")
-
 # TODO: remove non shared cereal and messaging
 cereal_objects = env.SharedObject([f'gen/cpp/{s}.c++' for s in schema_files])
 
diff --git a/cereal/car.capnp b/cereal/car.capnp
index 686e0e2df..d78acb28e 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -1,10 +1,6 @@
 using Cxx = import "./include/c++.capnp";
 $Cxx.namespace("cereal");
 
-using Java = import "./include/java.capnp";
-$Java.package("ai.comma.openpilot.cereal");
-$Java.outerClassname("Car");
-
 @0x8e2af1e708af8b8d;
 
 # ******* events causing controls state machine transition *******
@@ -88,7 +84,6 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     startupNoCar @76;
     startupNoControl @77;
     startupMaster @78;
-    startupFuzzyFingerprint @97;
     startupNoFw @104;
     fcw @79;
     steerSaturated @80;
@@ -130,6 +125,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
     neosUpdateRequiredDEPRECATED @88;
     modelLagWarningDEPRECATED @93;
     startupOneplusDEPRECATED @82;
+    startupFuzzyFingerprintDEPRECATED @97;
   }
 }
 
@@ -148,7 +144,7 @@ struct CarState {
   wheelSpeeds @2 :WheelSpeeds;
 
   # gas pedal, 0.0-1.0
-  gas @3 :Float32;        # this is user + computer
+  gas @3 :Float32;        # this is user pedal only
   gasPressed @4 :Bool;    # this is user pedal only
 
   # brake pedal, 0.0-1.0
@@ -379,14 +375,11 @@ struct CarParams {
   enableDsu @5 :Bool;        # driving support unit
   enableApgs @6 :Bool;       # advanced parking guidance system
   enableBsm @56 :Bool;       # blind spot monitoring
-  hasStockCamera @57 :Bool;  # factory LKAS/LDW camera is present
 
   minEnableSpeed @7 :Float32;
   minSteerSpeed @8 :Float32;
   maxSteeringAngleDeg @54 :Float32;
-  safetyModel @9 :SafetyModel;
-  safetyModelPassive @42 :SafetyModel = silent;
-  safetyParam @10 :Int16;
+  safetyConfigs @62 :List(SafetyConfig);
 
   steerMaxBP @11 :List(Float32);
   steerMaxV @12 :List(Float32);
@@ -419,9 +412,11 @@ struct CarParams {
   steerLimitTimer @47 :Float32;  # time before steerLimitAlert is issued
 
   vEgoStopping @29 :Float32; # Speed at which the car goes into stopping state
+  vEgoStarting @59 :Float32; # Speed at which the car goes into starting state
   directAccelControl @30 :Bool; # Does the car have direct accel control or just gas/brake
   stoppingControl @31 :Bool; # Does the car allows full control even at lows speeds when stopping
   startAccel @32 :Float32; # Required acceleraton to overcome creep braking
+  stopAccel @60 :Float32; # Required acceleraton to keep vehicle stationary
   steerRateCost @33 :Float32; # Lateral MPC cost on steering rate
   steerControlType @34 :SteerControlType;
   radarOffCan @35 :Bool; # True when radar objects aren't visible on CAN
@@ -430,7 +425,8 @@ struct CarParams {
   startingAccelRate @53 :Float32; # m/s^2/s while trying to start
 
   steerActuatorDelay @36 :Float32; # Steering wheel actuator delay in seconds
-  longitudinalActuatorDelay @58 :Float32; # Gas/Brake actuator delay in seconds
+  longitudinalActuatorDelayLowerBound @61 :Float32; # Gas/Brake actuator delay in seconds, lower bound
+  longitudinalActuatorDelayUpperBound @58 :Float32; # Gas/Brake actuator delay in seconds, upper bound
   openpilotLongitudinalControl @37 :Bool; # is openpilot doing the longitudinal control?
   carVin @38 :Text; # VIN number queried during fingerprinting
   dashcamOnly @41: Bool;
@@ -442,6 +438,11 @@ struct CarParams {
   fingerprintSource @49: FingerprintSource;
   networkLocation @50 :NetworkLocation;  # Where Panda/C2 is integrated into the car's CAN network
 
+  struct SafetyConfig {
+    safetyModel @0 :SafetyModel;
+    safetyParam @1 :Int16;
+  }
+
   struct LateralParams {
     torqueBP @0 :List(Int32);
     torqueV @1 :List(Int32);
@@ -520,6 +521,7 @@ struct CarParams {
     subaruLegacy @22;  # pre-Global platform
     hyundaiLegacy @23;
     hyundaiCommunity @24;
+    stellantis @25;
   }
 
   enum SteerControlType {
@@ -579,4 +581,8 @@ struct CarParams {
 
   enableCameraDEPRECATED @4 :Bool;
   isPandaBlackDEPRECATED @39: Bool;
+  hasStockCameraDEPRECATED @57 :Bool;
+  safetyParamDEPRECATED @10 :Int16;
+  safetyModelDEPRECATED @9 :SafetyModel;
+  safetyModelPassiveDEPRECATED @42 :SafetyModel = silent;
 }
diff --git a/cereal/include/java.capnp b/cereal/include/java.capnp
deleted file mode 100644
index cddf6eba3..000000000
--- a/cereal/include/java.capnp
+++ /dev/null
@@ -1,28 +0,0 @@
-# Copyright (c) 2013-2015 Sandstorm Development Group, Inc. and contributors
-# Licensed under the MIT License:
-#
-# Permission is hereby granted, free of charge, to any person obtaining a copy
-# of this software and associated documentation files (the "Software"), to deal
-# in the Software without restriction, including without limitation the rights
-# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-# copies of the Software, and to permit persons to whom the Software is
-# furnished to do so, subject to the following conditions:
-#
-# The above copyright notice and this permission notice shall be included in
-# all copies or substantial portions of the Software.
-#
-# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-# THE SOFTWARE.
-
-@0xc5f1af96651f70ea;
-
-annotation package @0x9ee4c8f803b3b596 (file) : Text;
-# Name of the package, such as "org.example.foo", in which the generated code will reside.
-
-annotation outerClassname @0x9b066bb4881f7cd3 (file) : Text;
-# Name of the outer class that will wrap the generated code.
diff --git a/cereal/legacy.capnp b/cereal/legacy.capnp
index 0534dabc1..9a4891681 100644
--- a/cereal/legacy.capnp
+++ b/cereal/legacy.capnp
@@ -1,10 +1,6 @@
 using Cxx = import "./include/c++.capnp";
 $Cxx.namespace("cereal");
 
-using Java = import "./include/java.capnp";
-$Java.package("ai.comma.openpilot.cereal");
-$Java.outerClassname("Legacy");
-
 @0x80ef1ec4889c2a63;
 
 # legacy.capnp: a home for deprecated structs
diff --git a/cereal/log.capnp b/cereal/log.capnp
index e87c41fe0..8c6245ab5 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -1,10 +1,6 @@
 using Cxx = import "./include/c++.capnp";
 $Cxx.namespace("cereal");
 
-using Java = import "./include/java.capnp";
-$Java.package("ai.comma.openpilot.cereal");
-$Java.outerClassname("Log");
-
 using Car = import "car.capnp";
 using Legacy = import "legacy.capnp";
 
@@ -293,15 +289,11 @@ struct DeviceState @0xa4d8b5af2aa492eb {
   networkType @22 :NetworkType;
   networkInfo @31 :NetworkInfo;
   networkStrength @24 :NetworkStrength;
-  offroadPowerUsageUwh @23 :UInt32;
-  carBatteryCapacityUwh @25 :UInt32;
+  lastAthenaPingTime @32 :UInt64;
 
-  fanSpeedPercentDesired @10 :UInt16;
   started @11 :Bool;
   startedMonoTime @13 :UInt64;
 
-  lastAthenaPingTime @32 :UInt64;
-
   # system utilization
   freeSpacePercent @7 :Float32;
   memoryUsagePercent @19 :Int8;
@@ -313,14 +305,21 @@ struct DeviceState @0xa4d8b5af2aa492eb {
   batteryCurrent @15 :Int32;
   chargingError @17 :Bool;
   chargingDisabled @18 :Bool;
+  offroadPowerUsageUwh @23 :UInt32;
+  carBatteryCapacityUwh @25 :UInt32;
 
   # device thermals
   cpuTempC @26 :List(Float32);
   gpuTempC @27 :List(Float32);
   memoryTempC @28 :Float32;
   ambientTempC @30 :Float32;
+  nvmeTempC @35 :List(Float32);
+  modemTempC @36 :List(Float32);
   thermalStatus @14 :ThermalStatus;
 
+  fanSpeedPercentDesired @10 :UInt16;
+  screenBrightnessPercent @37 :Int8;
+
   enum ThermalStatus {
     green @0;
     yellow @1;
@@ -371,20 +370,14 @@ struct DeviceState @0xa4d8b5af2aa492eb {
 }
 
 struct PandaState @0xa7649e2575e4591e {
-  # from can health
-  voltage @0 :UInt32;
-  current @1 :UInt32;
   ignitionLine @2 :Bool;
   controlsAllowed @3 :Bool;
   gasInterceptorDetected @4 :Bool;
-  hasGps @6 :Bool;
   canSendErrs @7 :UInt32;
   canFwdErrs @8 :UInt32;
   canRxErrs @19 :UInt32;
   gmlanSendErrs @9 :UInt32;
   pandaType @10 :PandaType;
-  fanSpeedRpm @11 :UInt16;
-  usbPowerMode @12 :UsbPowerMode;
   ignitionCan @13 :Bool;
   safetyModel @14 :Car.CarParams.SafetyModel;
   safetyParam @20 :Int16;
@@ -438,13 +431,6 @@ struct PandaState @0xa7649e2575e4591e {
     redPanda @7;
   }
 
-  enum UsbPowerMode {
-    none @0;
-    client @1;
-    cdp @2;
-    dcp @3;
-  }
-
   enum HarnessStatus {
     notConnected @0;
     normal @1;
@@ -452,6 +438,26 @@ struct PandaState @0xa7649e2575e4591e {
   }
 
   startedSignalDetectedDEPRECATED @5 :Bool;
+  voltageDEPRECATED @0 :UInt32;
+  currentDEPRECATED @1 :UInt32;
+  hasGpsDEPRECATED @6 :Bool;
+  fanSpeedRpmDEPRECATED @11 :UInt16;
+  usbPowerModeDEPRECATED @12 :PeripheralState.UsbPowerMode;
+}
+
+struct PeripheralState {
+  pandaType @0 :PandaState.PandaType;
+  voltage @1 :UInt32;
+  current @2 :UInt32;
+  fanSpeedRpm @3 :UInt16;
+  usbPowerMode @4 :UsbPowerMode;
+
+  enum UsbPowerMode @0xa8883583b32c9877 {
+    none @0;
+    client @1;
+    cdp @2;
+    dcp @3;
+  }
 }
 
 struct RadarState @0x9a185389d6fdd05f {
@@ -859,7 +865,7 @@ struct LateralPlan @0xe1e9318e2ae8b51e {
   desire @17 :Desire;
   laneChangeState @18 :LaneChangeState;
   laneChangeDirection @19 :LaneChangeDirection;
-
+  useLaneLines @29 :Bool;
 
   # desired curvatures over next 2.5s in rad/m
   psis @26 :List(Float32);
@@ -1248,6 +1254,7 @@ struct DriverState {
   distractedEyes @20 :Float32;
   eyesOnRoad @21 :Float32;
   phoneUse @22 :Float32;
+  occludedProb @23 :Float32;
 
   irPwrDEPRECATED @10 :Float32;
   descriptorDEPRECATED @1 :List(Float32);
@@ -1295,6 +1302,10 @@ struct LiveParametersData {
   yawRate @7 :Float32;
   posenetSpeed @8 :Float32;
   posenetValid @9 :Bool;
+  angleOffsetFastStd @10 :Float32;
+  angleOffsetAverageStd @11 :Float32;
+  stiffnessFactorStd @12 :Float32;
+  steerRatioStd @13 :Float32;
 }
 
 struct LiveMapDataDEPRECATED {
@@ -1344,6 +1355,7 @@ struct ManagerState {
     name @0 :Text;
     pid @1 :Int32;
     running @2 :Bool;
+    shouldBeRunning @4 :Bool;
     exitCode @3 :Int32;
   }
 }
@@ -1377,7 +1389,8 @@ struct Event {
     can @5 :List(CanData);
     controlsState @7 :ControlsState;
     sensorEvents @11 :List(SensorEventData);
-    pandaState @12 :PandaState;
+    pandaStates @81 :List(PandaState);
+    peripheralState @80 :PeripheralState;
     radarState @13 :RadarState;
     liveTracks @16 :List(LiveTracks);
     sendcan @17 :List(CanData);
@@ -1457,5 +1470,6 @@ struct Event {
     kalmanOdometryDEPRECATED @65 :Legacy.KalmanOdometry;
     gpsLocationDEPRECATED @21 :GpsLocationData;
     uiLayoutStateDEPRECATED @57 :Legacy.UiLayoutState;
+    pandaStateDEPRECATED @12 :PandaState;
   }
 }
diff --git a/cereal/services.py b/cereal/services.py
index 406d9d146..4cf10a344 100755
--- a/cereal/services.py
+++ b/cereal/services.py
@@ -28,14 +28,15 @@ services = {
   "deviceState": (True, 2., 1),
   "can": (True, 100.),
   "controlsState": (True, 100., 10),
-  "pandaState": (True, 2., 1),
+  "pandaStates": (True, 2., 1),
+  "peripheralState": (True, 2., 1),
   "radarState": (True, 20., 5),
   "roadEncodeIdx": (True, 20., 1),
   "liveTracks": (True, 20.),
-  "sendcan": (True, 100.),
+  "sendcan": (True, 100., 139),
   "logMessage": (True, 0.),
   "liveCalibration": (True, 4., 4),
-  "androidLog": (True, 0., 1),
+  "androidLog": (True, 0.),
   "carState": (True, 100., 10),
   "carControl": (True, 100., 10),
   "longitudinalPlan": (True, 20., 5),
diff --git a/cereal/visionipc/visionbuf_ion.cc b/cereal/visionipc/visionbuf_ion.cc
index 60f7c66e1..c367304e9 100644
--- a/cereal/visionipc/visionbuf_ion.cc
+++ b/cereal/visionipc/visionbuf_ion.cc
@@ -2,6 +2,7 @@
 #include <stdio.h>
 #include <string.h>
 #include <assert.h>
+#include <errno.h>
 #include <sys/mman.h>
 #include <sys/ioctl.h>
 #include <sys/types.h>
@@ -15,6 +16,16 @@
 
 #include "visionbuf.h"
 
+// keep trying if x gets interrupted by a signal
+#define HANDLE_EINTR(x)                                       \
+  ({                                                          \
+    decltype(x) ret;                                          \
+    int try_cnt = 0;                                          \
+    do {                                                      \
+      ret = (x);                                              \
+    } while (ret == -1 && errno == EINTR && try_cnt++ < 100); \
+    ret;                                                      \
+  })
 
 // just hard-code these for convenience
 // size_t device_page_size = 0;
@@ -47,12 +58,12 @@ void VisionBuf::allocate(size_t len) {
   ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
   ion_alloc.flags = ION_FLAG_CACHED;
 
-  err = ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc);
+  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_ALLOC, &ion_alloc));
   assert(err == 0);
 
   struct ion_fd_data ion_fd_data = {0};
   ion_fd_data.handle = ion_alloc.handle;
-  err = ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data);
+  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data));
   assert(err == 0);
 
   void *addr = mmap(NULL, ion_alloc.len,
@@ -78,7 +89,7 @@ void VisionBuf::import(){
   // Get handle
   struct ion_fd_data fd_data = {0};
   fd_data.fd = this->fd;
-  err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data);
+  err = HANDLE_EINTR(ioctl(ion_fd, ION_IOC_IMPORT, &fd_data));
   assert(err == 0);
 
   this->handle = fd_data.handle;
@@ -122,7 +133,7 @@ int VisionBuf::sync(int dir) {
      ION_IOC_INV_CACHES : ION_IOC_CLEAN_CACHES;
 
   custom_data.arg = (unsigned long)&flush_data;
-  return ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data);
+  return HANDLE_EINTR(ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data));
 }
 
 int VisionBuf::free() {
@@ -140,5 +151,5 @@ int VisionBuf::free() {
   if (err != 0) return err;
 
   struct ion_handle_data handle_data = {.handle = this->handle};
-  return ioctl(ion_fd, ION_IOC_FREE, &handle_data);
+  return HANDLE_EINTR(ioctl(ion_fd, ION_IOC_FREE, &handle_data));
 }
diff --git a/cereal/visionipc/visionipc.pxd b/cereal/visionipc/visionipc.pxd
index 5e2b3a3a1..5cb729f7e 100644
--- a/cereal/visionipc/visionipc.pxd
+++ b/cereal/visionipc/visionipc.pxd
@@ -36,3 +36,4 @@ cdef extern from "visionipc_client.h":
     VisionIpcClient(string, VisionStreamType, bool, void*, void*)
     VisionBuf * recv(VisionIpcBufExtra *, int)
     bool connect(bool)
+    bool is_connected()
diff --git a/cereal/visionipc/visionipc_client.cc b/cereal/visionipc/visionipc_client.cc
index 69ae8b133..da406067a 100644
--- a/cereal/visionipc/visionipc_client.cc
+++ b/cereal/visionipc/visionipc_client.cc
@@ -72,6 +72,7 @@ bool VisionIpcClient::connect(bool blocking){
     if (device_id) buffers[i].init_cl(device_id, ctx);
   }
 
+  close(socket_fd);
   connected = true;
   return true;
 }
diff --git a/cereal/visionipc/visionipc_client.h b/cereal/visionipc/visionipc_client.h
index 04b2db6b0..a6426ce92 100644
--- a/cereal/visionipc/visionipc_client.h
+++ b/cereal/visionipc/visionipc_client.h
@@ -29,4 +29,5 @@ public:
   ~VisionIpcClient();
   VisionBuf * recv(VisionIpcBufExtra * extra=nullptr, const int timeout_ms=100);
   bool connect(bool blocking=true);
+  bool is_connected() { return connected; }
 };
diff --git a/cereal/visionipc/visionipc_pyx.pyx b/cereal/visionipc/visionipc_pyx.pyx
index d1eef64ec..c0ba4c3ee 100644
--- a/cereal/visionipc/visionipc_pyx.pyx
+++ b/cereal/visionipc/visionipc_pyx.pyx
@@ -88,3 +88,6 @@ cdef class VisionIpcClient:
 
   def connect(self, bool blocking):
     return self.client.connect(blocking)
+
+  def is_connected(self):
+    return self.client.is_connected()
diff --git a/common/params.py b/common/params.py
index d0a37cce1..2cfca3712 100644
--- a/common/params.py
+++ b/common/params.py
@@ -6,19 +6,14 @@ assert put_nonblocking
 
 if __name__ == "__main__":
   import sys
-  from common.params_pyx import keys # pylint: disable=no-name-in-module, import-error
 
   params = Params()
+  key = sys.argv[1]
+  assert params.check_key(key), f"unknown param: {key}"
+
   if len(sys.argv) == 3:
-    name = sys.argv[1]
     val = sys.argv[2]
-    assert name.encode("utf-8") in keys.keys(), f"unknown param: {name}"
-    print(f"SET: {name} = {val}")
-    params.put(name, val)
+    print(f"SET: {key} = {val}")
+    params.put(key, val)
   elif len(sys.argv) == 2:
-    name = sys.argv[1]
-    assert name.encode("utf-8") in keys.keys(), f"unknown param: {name}"
-    print(f"GET: {name} = {params.get(name)}")
-  else:
-    for k in keys.keys():
-      print(f"GET: {k} = {params.get(k)}")
+    print(f"GET: {key} = {params.get(key)}")
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
index 29ce3277d..d71f865ca 100644
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@@ -131,7 +131,7 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=W, height=H):
   img_pts = np.array(img_pts)
   input_shape = img_pts.shape
   img_pts = np.atleast_2d(img_pts)
-  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1))))
+  img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1), dtype=img_pts.dtype)))
   img_pts_denormalized = img_pts.dot(intrinsics.T)
   img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan
   img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan
diff --git a/docs/CARS.md b/docs/CARS.md
new file mode 100644
index 000000000..9766a7264
--- /dev/null
+++ b/docs/CARS.md
@@ -0,0 +1,176 @@
+Supported Cars
+------
+
+| Make      | Model (US Market Reference)   | Supported Package | ACC              | No ACC accel below | No ALC below      |
+| ----------| ------------------------------| ------------------| -----------------| -------------------| ------------------|
+| Acura     | ILX 2016-19                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 25mph             |
+| Acura     | RDX 2016-18                   | AcuraWatch Plus   | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Acura     | RDX 2019-21                   | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Accord 2018-20                | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Accord Hybrid 2018-20         | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Civic Hatchback 2017-21       | Honda Sensing     | Stock            | 0mph               | 12mph             |
+| Honda     | Civic Coupe 2016-18           | Honda Sensing     | openpilot        | 0mph               | 12mph             |
+| Honda     | Civic Coupe 2019-20           | All               | Stock            | 0mph               | 2mph<sup>2</sup>  |
+| Honda     | Civic Sedan 2016-18           | Honda Sensing     | openpilot        | 0mph               | 12mph             |
+| Honda     | Civic Sedan 2019-20           | All               | Stock            | 0mph               | 2mph<sup>2</sup>  |
+| Honda     | CR-V 2015-16                  | Touring           | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | CR-V 2017-21                  | Honda Sensing     | Stock            | 0mph               | 12mph             |
+| Honda     | CR-V Hybrid 2017-2019         | Honda Sensing     | Stock            | 0mph               | 12mph             |
+| Honda     | e 2020                        | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Fit 2018-19                   | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | Freed 2020                    | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | HR-V 2019-20                  | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | Insight 2019-21               | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Inspire 2018                  | All               | Stock            | 0mph               | 3mph              |
+| Honda     | Odyssey 2018-20               | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 0mph              |
+| Honda     | Passport 2019                 | All               | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | Pilot 2016-21                 | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Honda     | Ridgeline 2017-21             | Honda Sensing     | openpilot        | 25mph<sup>1</sup>  | 12mph             |
+| Hyundai   | Palisade 2020-21              | All               | Stock            | 0mph               | 0mph              |
+| Hyundai   | Sonata 2020-21                | All               | Stock            | 0mph               | 0mph              |
+| Lexus     | CT Hybrid 2017-18             | LSS               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | ES 2019-21                    | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | ES Hybrid 2017-18             | LSS               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | ES Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | IS 2017-2019                  | All               | Stock            | 22mph              | 0mph              |
+| Lexus     | NX 2018                       | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | NX 2020                       | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | NX Hybrid 2018-19             | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | RX 2016-18                    | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | RX 2020-21                    | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | RX Hybrid 2016-19             | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Lexus     | RX Hybrid 2020-21             | All               | openpilot        | 0mph               | 0mph              |
+| Lexus     | UX Hybrid 2019-21             | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Alphard 2020                  | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Avalon 2016-21                | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Avalon Hybrid 2019-21         | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Camry 2018-20                 | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
+| Toyota    | Camry 2021                    | All               | openpilot        | 0mph<sup>4</sup>   | 0mph              |
+| Toyota    | Camry Hybrid 2018-20          | All               | Stock            | 0mph<sup>4</sup>   | 0mph              |
+| Toyota    | Camry Hybrid 2021-22          | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | C-HR 2017-20                  | All               | Stock            | 0mph               | 0mph              |
+| Toyota    | C-HR Hybrid 2017-19           | All               | Stock            | 0mph               | 0mph              |
+| Toyota    | Corolla 2017-19               | All               | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Corolla 2020-22               | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Corolla Hatchback 2019-22     | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Corolla Hybrid 2020-22        | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Highlander 2017-19            | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Highlander 2020-21            | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Highlander Hybrid 2017-19     | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Highlander Hybrid 2020-21     | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Mirai 2021                    | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Prius 2016-20                 | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Prius 2021                    | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Prius Prime 2017-20           | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Prius Prime 2021              | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Rav4 2016-18                  | TSS-P             | Stock<sup>3</sup>| 20mph<sup>1</sup>  | 0mph              |
+| Toyota    | Rav4 2019-21                  | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Rav4 Hybrid 2016-18           | TSS-P             | Stock<sup>3</sup>| 0mph               | 0mph              |
+| Toyota    | Rav4 Hybrid 2019-21           | All               | openpilot        | 0mph               | 0mph              |
+| Toyota    | Sienna 2018-20                | All               | Stock<sup>3</sup>| 0mph               | 0mph              |
+
+<sup>1</sup>[Comma Pedal](https://github.com/commaai/openpilot/wiki/comma-pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).*** <br />
+<sup>2</sup>2019 Honda Civic 1.6L Diesel Sedan does not have ALC below 12mph. <br />
+<sup>3</sup>When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).*** <br />
+<sup>4</sup>28mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control. <br />
+
+Community Maintained Cars and Features
+------
+
+| Make      | Model (US Market Reference)   | Supported Package | ACC              | No ACC accel below | No ALC below |
+| ----------| ------------------------------| ------------------| -----------------| -------------------| -------------|
+| Audi      | A3 2014-19                    | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Audi      | A3 Sportback e-tron 2017-18   | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Audi      | Q2 2018                       | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Audi      | S3 2015                       | ACC + Lane Assist | Stock            | 0mph               | 0mph         |
+| Buick     | Regal 2018<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Cadillac  | ATS 2018<sup>1</sup>          | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Chevrolet | Malibu 2017<sup>1</sup>       | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Chevrolet | Volt 2017-18<sup>1</sup>      | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Chrysler  | Pacifica 2017-18              | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
+| Chrysler  | Pacifica 2020                 | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
+| Chrysler  | Pacifica Hybrid 2017-18       | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
+| Chrysler  | Pacifica Hybrid 2019-21       | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
+| Genesis   | G70 2018                      | All               | Stock            | 0mph               | 0mph         |
+| Genesis   | G80 2018                      | All               | Stock            | 0mph               | 0mph         |
+| Genesis   | G90 2018                      | All               | Stock            | 0mph               | 0mph         |
+| GMC       | Acadia 2018<sup>1</sup>       | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Holden    | Astra 2017<sup>1</sup>        | Adaptive Cruise   | openpilot        | 0mph               | 7mph         |
+| Hyundai   | Elantra 2017-19               | SCC + LKAS        | Stock            | 19mph              | 34mph        |
+| Hyundai   | Elantra 2021                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Elantra Hybrid 2021           | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Genesis 2015-16               | SCC + LKAS        | Stock            | 19mph              | 37mph        |
+| Hyundai   | Ioniq Electric 2019           | SCC + LKAS        | Stock            | 0mph               | 32mph        |
+| Hyundai   | Ioniq Electric 2020           | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Ioniq Hybrid 2017-19          | SCC + LKAS        | Stock            | 0mph               | 32mph        |
+| Hyundai   | Ioniq Hybrid 2020-22          | SCC + LFA         | Stock            | 0mph               | 0mph         |
+| Hyundai   | Ioniq PHEV 2020-21            | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Kona 2020                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Kona EV 2018-19               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Kona Hybrid 2020              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Santa Fe 2019-20              | All               | Stock            | 0mph               | 0mph         |
+| Hyundai   | Santa Fe 2021-22              | All               | Stock            | 0mph               | 0mph         |
+| Hyundai   | Sonata 2018-2019              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Hyundai   | Sonata Hybrid 2021-22         | All               | Stock            | 0mph               | 0mph         |
+| Hyundai   | Veloster 2019-20              | SCC + LKAS        | Stock            | 5mph               | 0mph         |
+| Jeep      | Grand Cherokee 2016-18        | Adaptive Cruise   | Stock            | 0mph               | 9mph         |
+| Jeep      | Grand Cherokee 2019-20        | Adaptive Cruise   | Stock            | 0mph               | 39mph        |
+| Kia       | Ceed 2019                     | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Forte 2018-21                 | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | K5 2021                       | SCC + LFA         | Stock            | 0mph               | 0mph         |
+| Kia       | Niro EV 2019-21               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Niro Hybrid 2021              | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Niro PHEV 2019                | SCC + LKAS        | Stock            | 10mph              | 32mph        |
+| Kia       | Optima 2017                   | SCC + LKAS        | Stock            | 0mph               | 32mph        |
+| Kia       | Optima 2019                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Seltos 2021                   | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Sorento 2018-19               | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Stinger 2018                  | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Kia       | Telluride 2020                | SCC + LKAS        | Stock            | 0mph               | 0mph         |
+| Nissan    | Altima 2019-20                | ProPILOT          | Stock            | 0mph               | 0mph         |
+| Nissan    | Leaf 2018-22                  | ProPILOT          | Stock            | 0mph               | 0mph         |
+| Nissan    | Rogue 2018-20                 | ProPILOT          | Stock            | 0mph               | 0mph         |
+| Nissan    | X-Trail 2017                  | ProPILOT          | Stock            | 0mph               | 0mph         |
+| SEAT      | Ateca 2018                    | Driver Assistance | Stock            | 0mph               | 0mph         |
+| SEAT      | Leon 2014-2020                | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Kamiq 2021<sup>2</sup>        | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Karoq 2019                    | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Kodiaq 2018-19                | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Octavia 2015, 2018-19         | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Octavia RS 2016               | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Scala 2020                    | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Škoda     | Superb 2015-18                | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Subaru    | Ascent 2019                   | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Crosstrek 2018-19             | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Forester 2019-21              | EyeSight          | Stock            | 0mph               | 0mph         |
+| Subaru    | Impreza 2017-19               | EyeSight          | Stock            | 0mph               | 0mph         |
+| Volkswagen| Arteon 2021<sup>4</sup>       | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Atlas 2018-19                 | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| California 2021<sup>4</sup>   | Driver Assistance | Stock            | 0mph               | 32mph        |
+| Volkswagen| e-Golf 2014, 2019-20          | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf 2015-20                  | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf Alltrack 2017-18         | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf GTE 2016                 | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf GTI 2018-20              | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf R 2016-19                | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf SportsVan 2016           | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Golf SportWagen 2015          | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Jetta 2018-20                 | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Jetta GLI 2021                | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Passat 2016-18<sup>3</sup>    | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| T-Cross 2021<sup>4</sup>      | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Taos 2022<sup>4</sup>         | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Tiguan 2020                   | Driver Assistance | Stock            | 0mph               | 0mph         |
+| Volkswagen| Touran 2017                   | Driver Assistance | Stock            | 0mph               | 0mph         |
+
+<sup>1</sup>Requires an [OBD-II car harness](https://comma.ai/shop/products/comma-car-harness) and [community built ASCM harness](https://github.com/commaai/openpilot/wiki/GM#hardware). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).*** <br />
+<sup>2</sup>Not including the China market Kamiq, which is based on the (currently) unsupported PQ34 platform. <br />
+<sup>3</sup>Not including the USA/China market Passat, which is based on the (currently) unsupported PQ35/NMS platform. <br />
+<sup>4</sup>Model-years 2021 and beyond may have a new camera harness design, which isn't yet available from the comma store. Before ordering,
+remove the Lane Assist camera cover and check to see if the connector is black (older design) or light brown (newer design). For the newer design,
+in the interim, choose "VW J533 Development" from the vehicle drop-down for a harness that integrates at the CAN gateway inside the dashboard.<br />
+Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`.
+
+To promote a car from community maintained, it must meet a few requirements. We must own one from the brand, we must sell the harness for it, has full ISO26262 in both panda and openpilot, there must be a path forward for longitudinal control, it must have AEB still enabled, and it must support fingerprinting 2.0
+
+Although they're not upstream, the community has openpilot running on other makes and models. See the 'Community Supported Models' section of each make [on our wiki](https://wiki.comma.ai/).
diff --git a/CONTRIBUTING.md b/docs/CONTRIBUTING.md
similarity index 96%
rename from CONTRIBUTING.md
rename to docs/CONTRIBUTING.md
index bcf05016c..8dcdeee50 100644
--- a/CONTRIBUTING.md
+++ b/docs/CONTRIBUTING.md
@@ -6,6 +6,7 @@ Most open source development activity is coordinated through our [GitHub Discuss
 
 ## Getting Started
 
+ * Setup your [development environment](../tools/)
  * Join our [Discord](https://discord.comma.ai)
  * Make sure you have a [GitHub account](https://github.com/signup/free)
  * Fork [our repositories](https://github.com/commaai) on GitHub
@@ -20,7 +21,7 @@ All PRs and commits are automatically checked by GitHub Actions. Check out `.git
 
 Code is automatically checked for style by GitHub Actions as part of the automated tests. You can also run these tests yourself by running `pre-commit run --all`.
 
-## Car Ports (openpilot)
+## Car Ports
 
 We've released a [Model Port guide](https://blog.comma.ai/openpilot-port-guide-for-toyota-models/) for porting to Toyota/Lexus models.
 
diff --git a/docs/INTEGRATION.md b/docs/INTEGRATION.md
new file mode 100644
index 000000000..a6b8194aa
--- /dev/null
+++ b/docs/INTEGRATION.md
@@ -0,0 +1,73 @@
+Integration with Stock Features
+------
+
+In all supported cars:
+* Stock Lane Keep Assist (LKA) and stock ALC are replaced by openpilot ALC, which only functions when openpilot is engaged by the user.
+* Stock LDW is replaced by openpilot LDW.
+
+Additionally, on specific supported cars (see ACC column in [supported cars](CARS.md)):
+* Stock ACC is replaced by openpilot ACC.
+* openpilot FCW operates in addition to stock FCW.
+
+openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning.
+
+Limitations of openpilot ALC and LDW
+------
+
+openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times.
+
+While changing lanes, openpilot is not capable of looking next to you or checking your blind spot. Only nudge the wheel to initiate a lane change after you have confirmed it's safe to do so.
+
+Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to:
+
+* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
+* The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc.
+* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
+* The device is mounted incorrectly.
+* When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce.
+* In the presence of restricted lanes or construction zones.
+* When driving on highly banked roads or in presence of strong cross-wind.
+* Extremely hot or cold temperatures.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* Driving on hills, narrow, or winding roads.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
+
+Limitations of openpilot ACC and FCW
+------
+
+openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times.
+
+Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to:
+
+* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation.
+* The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc.
+* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle.
+* The device is mounted incorrectly.
+* Approaching a toll booth, a bridge or a large metal plate.
+* When driving on roads with pedestrians, cyclists, etc...
+* In presence of traffic signs or stop lights, which are not detected by openpilot at this time.
+* When the posted speed limit is below the user selected set speed. openpilot does not detect speed limits at this time.
+* In presence of vehicles in the same lane that are not moving.
+* When abrupt braking maneuvers are required. openpilot is designed to be limited in the amount of deceleration and acceleration that it can produce.
+* When surrounding vehicles perform close cut-ins from neighbor lanes.
+* Driving on hills, narrow, or winding roads.
+* Extremely hot or cold temperatures.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* Interference from other equipment that generates radar waves.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times.
+
+Limitations of openpilot DM
+------
+
+openpilot DM should not be considered an exact measurement of the alertness of the driver.
+
+Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to:
+
+* Low light conditions, such as driving at night or in dark tunnels.
+* Bright light (due to oncoming headlights, direct sunlight, etc.).
+* The driver's face is partially or completely outside field of view of the driver facing camera.
+* The driver facing camera is obstructed, covered, or damaged.
+
+The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention.
diff --git a/SAFETY.md b/docs/SAFETY.md
similarity index 100%
rename from SAFETY.md
rename to docs/SAFETY.md
diff --git a/installer/updater/Makefile b/installer/updater/Makefile
deleted file mode 100644
index 54bd7c9df..000000000
--- a/installer/updater/Makefile
+++ /dev/null
@@ -1,99 +0,0 @@
-CC = clang
-CXX = clang++
-
-PHONELIBS = ../../phonelibs
-
-WARN_FLAGS = -Werror=implicit-function-declaration \
-             -Werror=incompatible-pointer-types \
-             -Werror=int-conversion \
-             -Werror=return-type \
-             -Werror=format-extra-args
-
-CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
-CXXFLAGS = -std=c++1z -g -fPIC -O2 $(WARN_FLAGS)
-
-CURL_FLAGS = -I$(PHONELIBS)/curl/include
-CURL_LIBS = $(PHONELIBS)/curl/lib/libcurl.a \
-            $(PHONELIBS)/zlib/lib/libz.a
-
-BORINGSSL_FLAGS = -I$(PHONELIBS)/boringssl/include
-BORINGSSL_LIBS = $(PHONELIBS)/boringssl/lib/libssl_static.a \
-                 $(PHONELIBS)/boringssl/lib/libcrypto_static.a \
-
-NANOVG_FLAGS = -I$(PHONELIBS)/nanovg
-
-JSON11_FLAGS = -I$(PHONELIBS)/json11
-
-OPENGL_LIBS = -lGLESv3
-
-FRAMEBUFFER_LIBS = -lutils -lgui -lEGL
-
-.PHONY: all
-all: updater
-
-OBJS = opensans_regular.ttf.o \
-			 opensans_semibold.ttf.o \
-			 opensans_bold.ttf.o \
-       ../../selfdrive/common/util.o \
-       ../../selfdrive/common/touch.o \
-       ../../selfdrive/common/framebuffer.o \
-       $(PHONELIBS)/json11/json11.o \
-       $(PHONELIBS)/nanovg/nanovg.o
-
-DEPS := $(OBJS:.o=.d)
-
-updater: updater.o $(OBJS)
-	@echo "[ LINK ] $@"
-	$(CXX) $(CPPFLAGS) -fPIC -o 'updater' $^ \
-    $(FRAMEBUFFER_LIBS) \
-    $(CURL_LIBS) \
-    $(BORINGSSL_LIBS) \
-    -L/system/vendor/lib64 \
-    $(OPENGL_LIBS) \
-    -lcutils -lm -llog
-	strip updater
-
-opensans_regular.ttf.o: ../../selfdrive/assets/fonts/opensans_regular.ttf
-	@echo "[ bin2o ] $@"
-	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-opensans_bold.ttf.o: ../../selfdrive/assets/fonts/opensans_bold.ttf
-	@echo "[ bin2o ] $@"
-	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-opensans_semibold.ttf.o: ../../selfdrive/assets/fonts/opensans_semibold.ttf
-	@echo "[ bin2o ] $@"
-	cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)'
-
-%.o: %.c
-	mkdir -p $(@D)
-	@echo "[ CC ] $@"
-	$(CC) $(CPPFLAGS) $(CFLAGS) \
-          -I../.. \
-          -I$(PHONELIBS)/android_frameworks_native/include \
-          -I$(PHONELIBS)/android_system_core/include \
-          -I$(PHONELIBS)/android_hardware_libhardware/include \
-          $(NANOVG_FLAGS) \
-          -c -o '$@' '$<'
-
-%.o: %.cc
-	mkdir -p $(@D)
-	@echo "[ CXX ] $@"
-	$(CXX) $(CPPFLAGS) $(CXXFLAGS) \
-           -I../../selfdrive \
-           -I../../ \
-           -I$(PHONELIBS)/android_frameworks_native/include \
-           -I$(PHONELIBS)/android_system_core/include \
-           -I$(PHONELIBS)/android_hardware_libhardware/include \
-           $(NANOVG_FLAGS) \
-           $(JSON11_FLAGS) \
-           $(CURL_FLAGS) \
-           $(BORINGSSL_FLAGS) \
-           -c -o '$@' '$<'
-
-
-.PHONY: clean
-clean:
-	rm -f $(OBJS) $(DEPS)
-
--include $(DEPS)
diff --git a/installer/updater/updater b/installer/updater/updater
index d833cb782..8bf40708a 100755
Binary files a/installer/updater/updater and b/installer/updater/updater differ
diff --git a/installer/updater/updater.cc b/installer/updater/updater.cc
deleted file mode 100644
index d1a694ef9..000000000
--- a/installer/updater/updater.cc
+++ /dev/null
@@ -1,803 +0,0 @@
-#include <sys/stat.h>
-#include <sys/statvfs.h>
-#include <unistd.h>
-
-#include <cassert>
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <fstream>
-#include <iostream>
-#include <memory>
-#include <mutex>
-#include <sstream>
-#include <string>
-#include <thread>
-
-#include <curl/curl.h>
-#include <openssl/sha.h>
-#include <EGL/egl.h>
-#include <EGL/eglext.h>
-#include <GLES3/gl3.h>
-#include "nanovg.h"
-#define NANOVG_GLES3_IMPLEMENTATION
-#include "json11.hpp"
-#include "nanovg_gl.h"
-#include "nanovg_gl_utils.h"
-
-#include "selfdrive/common/framebuffer.h"
-#include "selfdrive/common/touch.h"
-#include "selfdrive/common/util.h"
-
-#define USER_AGENT "NEOSUpdater-0.2"
-
-#define MANIFEST_URL_NEOS_STAGING "https://github.com/commaai/eon-neos/raw/master/update.staging.json"
-#define MANIFEST_URL_NEOS_LOCAL "http://192.168.5.1:8000/neosupdate/update.local.json"
-#define MANIFEST_URL_NEOS "https://github.com/commaai/eon-neos/raw/master/update.json"
-const char *manifest_url = MANIFEST_URL_NEOS;
-
-#define RECOVERY_DEV "/dev/block/bootdevice/by-name/recovery"
-#define RECOVERY_COMMAND "/cache/recovery/command"
-
-#define UPDATE_DIR "/data/neoupdate"
-
-extern const uint8_t bin_opensans_regular[] asm("_binary_opensans_regular_ttf_start");
-extern const uint8_t bin_opensans_regular_end[] asm("_binary_opensans_regular_ttf_end");
-extern const uint8_t bin_opensans_semibold[] asm("_binary_opensans_semibold_ttf_start");
-extern const uint8_t bin_opensans_semibold_end[] asm("_binary_opensans_semibold_ttf_end");
-extern const uint8_t bin_opensans_bold[] asm("_binary_opensans_bold_ttf_start");
-extern const uint8_t bin_opensans_bold_end[] asm("_binary_opensans_bold_ttf_end");
-
-namespace {
-
-std::string sha256_file(std::string fn, size_t limit=0) {
-  SHA256_CTX ctx;
-  SHA256_Init(&ctx);
-
-  FILE *file = fopen(fn.c_str(), "rb");
-  if (!file) return "";
-
-  const size_t buf_size = 8192;
-  std::unique_ptr<char[]> buffer( new char[ buf_size ] );
-
-  bool read_limit = (limit != 0);
-  while (true) {
-    size_t read_size = buf_size;
-    if (read_limit) read_size = std::min(read_size, limit);
-    size_t bytes_read = fread(buffer.get(), 1, read_size, file);
-    if (!bytes_read) break;
-
-    SHA256_Update(&ctx, buffer.get(), bytes_read);
-
-    if (read_limit) {
-      limit -= bytes_read;
-      if (limit == 0) break;
-    }
-  }
-
-  uint8_t hash[SHA256_DIGEST_LENGTH];
-  SHA256_Final(hash, &ctx);
-
-  fclose(file);
-
-  return util::tohex(hash, sizeof(hash));
-}
-
-size_t download_string_write(void *ptr, size_t size, size_t nmeb, void *up) {
-  size_t sz = size * nmeb;
-  ((std::string*)up)->append((char*)ptr, sz);
-  return sz;
-}
-
-std::string download_string(CURL *curl, std::string url) {
-  std::string os;
-
-  curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
-  curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1);
-  curl_easy_setopt(curl, CURLOPT_NOSIGNAL, 0);
-  curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT);
-  curl_easy_setopt(curl, CURLOPT_FAILONERROR, 1);
-  curl_easy_setopt(curl, CURLOPT_RESUME_FROM, 0);
-
-  curl_easy_setopt(curl, CURLOPT_NOPROGRESS, 1);
-
-  curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, download_string_write);
-  curl_easy_setopt(curl, CURLOPT_WRITEDATA, &os);
-  CURLcode res = curl_easy_perform(curl);
-  if (res != CURLE_OK) {
-    return "";
-  }
-
-  return os;
-}
-
-size_t download_file_write(void *ptr, size_t size, size_t nmeb, void *up) {
-  return fwrite(ptr, size, nmeb, (FILE*)up);
-}
-
-int battery_capacity() {
-  std::string bat_cap_s = util::read_file("/sys/class/power_supply/battery/capacity");
-  return atoi(bat_cap_s.c_str());
-}
-
-int battery_current() {
-  std::string current_now_s = util::read_file("/sys/class/power_supply/battery/current_now");
-  return atoi(current_now_s.c_str());
-}
-
-bool check_battery() {
-  int bat_cap = battery_capacity();
-  int current_now = battery_current();
-  return bat_cap > 35 || (current_now < 0 && bat_cap > 10);
-}
-
-bool check_space() {
-  struct statvfs stat;
-  if (statvfs("/data/", &stat) != 0) {
-    return false;
-  }
-  size_t space = stat.f_bsize * stat.f_bavail;
-  return space > 2000000000ULL; // 2GB
-}
-
-static void start_settings_activity(const char* name) {
-  char launch_cmd[1024];
-  snprintf(launch_cmd, sizeof(launch_cmd),
-           "am start -W --ez :settings:show_fragment_as_subsetting true -n 'com.android.settings/.%s'", name);
-  system(launch_cmd);
-}
-
-bool is_settings_active() {
-  FILE *fp;
-  char sys_output[4096];
-
-  fp = popen("/bin/dumpsys window windows", "r");
-  if (fp == NULL) {
-    return false;
-  }
-
-  bool active = false;
-  while (fgets(sys_output, sizeof(sys_output), fp) != NULL) {
-    if (strstr(sys_output, "mCurrentFocus=null")  != NULL) {
-      break;
-    }
-
-    if (strstr(sys_output, "mCurrentFocus=Window") != NULL) {
-      active = true;
-      break;
-    }
-  }
-
-  pclose(fp);
-
-  return active;
-}
-
-struct Updater {
-  bool do_exit = false;
-
-  TouchState touch;
-
-  int fb_w, fb_h;
-
-  std::unique_ptr<FrameBuffer> fb;
-  NVGcontext *vg = NULL;
-  int font_regular;
-  int font_semibold;
-  int font_bold;
-
-  std::thread update_thread_handle;
-
-  std::mutex lock;
-
-  enum UpdateState {
-    CONFIRMATION,
-    LOW_BATTERY,
-    RUNNING,
-    ERROR,
-  };
-  UpdateState state;
-
-  std::string progress_text;
-  float progress_frac;
-
-  std::string error_text;
-
-  std::string low_battery_text;
-  std::string low_battery_title;
-  std::string low_battery_context;
-  std::string battery_cap_text;
-  int min_battery_cap = 35;
-
-  // button
-  int b_x, b_w, b_y, b_h;
-  int balt_x;
-
-  // download stage writes these for the installation stage
-  int recovery_len;
-  std::string recovery_hash;
-  std::string recovery_fn;
-  std::string ota_fn;
-
-  CURL *curl = NULL;
-
-  void ui_init() {
-    touch_init(&touch);
-
-    fb = std::make_unique<FrameBuffer>("updater", 0x00001000, false, &fb_w, &fb_h);
-
-    fb->set_power(HWC_POWER_MODE_NORMAL);
-
-    vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
-    assert(vg);
-
-    font_regular = nvgCreateFontMem(vg, "opensans_regular", (unsigned char*)bin_opensans_regular, (bin_opensans_regular_end - bin_opensans_regular), 0);
-    assert(font_regular >= 0);
-
-    font_semibold = nvgCreateFontMem(vg, "opensans_semibold", (unsigned char*)bin_opensans_semibold, (bin_opensans_semibold_end - bin_opensans_semibold), 0);
-    assert(font_semibold >= 0);
-
-    font_bold = nvgCreateFontMem(vg, "opensans_bold", (unsigned char*)bin_opensans_bold, (bin_opensans_bold_end - bin_opensans_bold), 0);
-    assert(font_bold >= 0);
-
-    b_w = 640;
-    balt_x = 200;
-    b_x = fb_w-b_w-200;
-    b_y = 720;
-    b_h = 220;
-
-    if (download_stage(true)) {
-      state = RUNNING;
-      update_thread_handle = std::thread(&Updater::run_stages, this);
-    } else {
-      state = CONFIRMATION;
-    }
-  }
-
-  int download_file_xferinfo(curl_off_t dltotal, curl_off_t dlno,
-                             curl_off_t ultotal, curl_off_t ulnow) {
-    {
-      std::lock_guard<std::mutex> guard(lock);
-      if (dltotal != 0) {
-        progress_frac = (float) dlno / dltotal;
-      }
-    }
-    // printf("info: %ld %ld %f\n", dltotal, dlno, progress_frac);
-    return 0;
-  }
-
-  bool download_file(std::string url, std::string out_fn) {
-    FILE *of = fopen(out_fn.c_str(), "ab");
-    assert(of);
-
-    CURLcode res;
-    long last_resume_from = 0;
-
-    fseek(of, 0, SEEK_END);
-
-    int tries = 4;
-
-    bool ret = false;
-
-    while (true) {
-      long resume_from = ftell(of);
-
-      curl_easy_setopt(curl, CURLOPT_URL, url.c_str());
-      curl_easy_setopt(curl, CURLOPT_FOLLOWLOCATION, 1);
-      curl_easy_setopt(curl, CURLOPT_NOSIGNAL, 0);
-      curl_easy_setopt(curl, CURLOPT_USERAGENT, USER_AGENT);
-      curl_easy_setopt(curl, CURLOPT_FAILONERROR, 1);
-      curl_easy_setopt(curl, CURLOPT_RESUME_FROM, resume_from);
-
-      curl_easy_setopt(curl, CURLOPT_WRITEFUNCTION, download_file_write);
-      curl_easy_setopt(curl, CURLOPT_WRITEDATA, of);
-
-      curl_easy_setopt(curl, CURLOPT_NOPROGRESS, 0);
-
-      curl_easy_setopt(curl, CURLOPT_XFERINFODATA, this);
-      curl_easy_setopt(curl, CURLOPT_XFERINFOFUNCTION, &Updater::download_file_xferinfo);
-
-      CURLcode res = curl_easy_perform(curl);
-
-      long response_code = 0;
-      curl_easy_getinfo(curl, CURLINFO_RESPONSE_CODE, &response_code);
-
-      // double content_length = 0.0;
-      // curl_easy_getinfo(curl, CURLINFO_CONTENT_LENGTH_DOWNLOAD, &content_length);
-
-      printf("download %s res %d, code %ld, resume from %ld\n", url.c_str(), res, response_code, resume_from);
-      if (res == CURLE_OK) {
-        ret = true;
-        break;
-      } else if (res == CURLE_HTTP_RETURNED_ERROR && response_code == 416) {
-        // failed because the file is already complete?
-        ret = true;
-        break;
-      } else if (resume_from == last_resume_from) {
-        // failed and dind't make make forward progress. only retry a couple times
-        tries--;
-        if (tries <= 0) {
-          break;
-        }
-      }
-      last_resume_from = resume_from;
-    }
-    // printf("res %d\n", res);
-
-    // printf("- %ld %f\n", response_code, content_length);
-
-    fclose(of);
-
-    return ret;
-  }
-
-  void set_progress(std::string text) {
-    std::lock_guard<std::mutex> guard(lock);
-    progress_text = text;
-  }
-
-  void set_error(std::string text) {
-    std::lock_guard<std::mutex> guard(lock);
-    error_text = text;
-    state = ERROR;
-  }
-
-  void set_battery_low() {
-    std::lock_guard<std::mutex> guard(lock);
-    state = LOW_BATTERY;
-  }
-
-  void set_running() {
-    std::lock_guard<std::mutex> guard(lock);
-    state = RUNNING;
-  }
-
-  std::string download(std::string url, std::string hash, std::string name, bool dry_run) {
-    std::string out_fn = UPDATE_DIR "/" + util::base_name(url);
-
-    std::string fn_hash = sha256_file(out_fn);
-    if (dry_run) {
-      return (hash.compare(fn_hash) != 0) ? "" : out_fn;
-    }
-
-    // start or resume downloading if hash doesn't match
-    if (hash.compare(fn_hash) != 0) {
-      set_progress("Downloading " + name + "...");
-      bool r = download_file(url, out_fn);
-      if (!r) {
-        set_error("failed to download " + name);
-        unlink(out_fn.c_str());
-        return "";
-      }
-      fn_hash = sha256_file(out_fn);
-    }
-
-    set_progress("Verifying " + name + "...");
-    printf("got %s hash: %s\n", name.c_str(), hash.c_str());
-    if (fn_hash != hash) {
-      set_error(name + " was corrupt");
-      unlink(out_fn.c_str());
-      return "";
-    }
-    return out_fn;
-  }
-
-  bool download_stage(bool dry_run = false) {
-    curl = curl_easy_init();
-    assert(curl);
-
-    // ** quick checks before download **
-
-    if (!check_space()) {
-      if (!dry_run) set_error("2GB of free space required to update");
-      return false;
-    }
-
-    mkdir(UPDATE_DIR, 0777);
-
-    set_progress("Finding latest version...");
-    std::string manifest_s = download_string(curl, manifest_url);
-    printf("manifest: %s\n", manifest_s.c_str());
-
-    std::string err;
-    auto manifest = json11::Json::parse(manifest_s, err);
-    if (manifest.is_null() || !err.empty()) {
-      set_error("failed to load update manifest");
-      return false;
-    }
-
-    std::string ota_url = manifest["ota_url"].string_value();
-    std::string ota_hash = manifest["ota_hash"].string_value();
-
-    std::string recovery_url = manifest["recovery_url"].string_value();
-    recovery_hash = manifest["recovery_hash"].string_value();
-    recovery_len = manifest["recovery_len"].int_value();
-
-    // std::string installer_url = manifest["installer_url"].string_value();
-    // std::string installer_hash = manifest["installer_hash"].string_value();
-
-    if (ota_url.empty() || ota_hash.empty()) {
-      set_error("invalid update manifest");
-      return false;
-    }
-
-    // std::string installer_fn = download(installer_url, installer_hash, "installer");
-    // if (installer_fn.empty()) {
-    //   //error'd
-    //   return;
-    // }
-
-    // ** handle recovery download **
-    if (recovery_url.empty() || recovery_hash.empty() || recovery_len == 0) {
-      set_progress("Skipping recovery flash...");
-    } else {
-      // only download the recovery if it differs from what's flashed
-      set_progress("Checking recovery...");
-      std::string existing_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
-      printf("existing recovery hash: %s\n", existing_recovery_hash.c_str());
-
-      if (existing_recovery_hash != recovery_hash) {
-        recovery_fn = download(recovery_url, recovery_hash, "recovery", dry_run);
-        if (recovery_fn.empty()) {
-          // error'd
-          return false;
-        }
-      }
-    }
-
-    // ** handle ota download **
-    ota_fn = download(ota_url, ota_hash, "update", dry_run);
-    if (ota_fn.empty()) {
-      //error'd
-      return false;
-    }
-
-    // download sucessful
-    return true;
-  }
-
-  // thread that handles downloading and installing the update
-  void run_stages() {
-    printf("run_stages start\n");
-
-
-    // ** download update **
-
-    if (!check_battery()) {
-      set_battery_low();
-      int battery_cap = battery_capacity();
-      while(battery_cap < min_battery_cap) {
-        battery_cap = battery_capacity();
-        battery_cap_text = std::to_string(battery_cap);
-        util::sleep_for(1000);
-      }
-      set_running();
-    }
-
-    bool sucess = download_stage();
-    if (!sucess) {
-      return;
-    }
-
-    // ** install update **
-
-    if (!check_battery()) {
-      set_battery_low();
-      int battery_cap = battery_capacity();
-      while(battery_cap < min_battery_cap) {
-        battery_cap = battery_capacity();
-        battery_cap_text = std::to_string(battery_cap);
-        util::sleep_for(1000);
-      }
-      set_running();
-    }
-
-    if (!recovery_fn.empty()) {
-      // flash recovery
-      set_progress("Flashing recovery...");
-
-      FILE *flash_file = fopen(recovery_fn.c_str(), "rb");
-      if (!flash_file) {
-        set_error("failed to flash recovery");
-        return;
-      }
-
-      FILE *recovery_dev = fopen(RECOVERY_DEV, "w+b");
-      if (!recovery_dev) {
-        fclose(flash_file);
-        set_error("failed to flash recovery");
-        return;
-      }
-
-      const size_t buf_size = 4096;
-      std::unique_ptr<char[]> buffer( new char[ buf_size ] );
-
-      while (true) {
-        size_t bytes_read = fread(buffer.get(), 1, buf_size, flash_file);
-        if (!bytes_read) break;
-
-        size_t bytes_written = fwrite(buffer.get(), 1, bytes_read, recovery_dev);
-        if (bytes_read != bytes_written) {
-          fclose(recovery_dev);
-          fclose(flash_file);
-          set_error("failed to flash recovery: write failed");
-          return;
-        }
-      }
-
-      fclose(recovery_dev);
-      fclose(flash_file);
-
-      set_progress("Verifying flash...");
-      std::string new_recovery_hash = sha256_file(RECOVERY_DEV, recovery_len);
-      printf("new recovery hash: %s\n", new_recovery_hash.c_str());
-
-      if (new_recovery_hash != recovery_hash) {
-        set_error("recovery flash corrupted");
-        return;
-      }
-
-    }
-
-    // write arguments to recovery
-    FILE *cmd_file = fopen(RECOVERY_COMMAND, "wb");
-    if (!cmd_file) {
-      set_error("failed to reboot into recovery");
-      return;
-    }
-    fprintf(cmd_file, "--update_package=%s\n", ota_fn.c_str());
-    fclose(cmd_file);
-
-    set_progress("Rebooting");
-
-    // remove the continue.sh so we come back into the setup.
-    // maybe we should go directly into the installer, but what if we don't come back with internet? :/
-    //unlink("/data/data/com.termux/files/continue.sh");
-
-    // TODO: this should be generic between android versions
-    // IPowerManager.reboot(confirm=false, reason="recovery", wait=true)
-    system("service call power 16 i32 0 s16 recovery i32 1");
-    while (true) pause();
-
-    // execl("/system/bin/reboot", "recovery");
-    // set_error("failed to reboot into recovery");
-  }
-
-  void draw_ack_screen(const char *title, const char *message, const char *button, const char *altbutton) {
-    nvgFillColor(vg, nvgRGBA(255,255,255,255));
-    nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
-
-    nvgFontFace(vg, "opensans_bold");
-    nvgFontSize(vg, 120.0f);
-    nvgTextBox(vg, 110, 220, fb_w-240, title, NULL);
-
-    nvgFontFace(vg, "opensans_regular");
-    nvgFontSize(vg, 86.0f);
-    nvgTextBox(vg, 130, 380, fb_w-260, message, NULL);
-
-    // draw button
-    if (button) {
-      nvgBeginPath(vg);
-      nvgFillColor(vg, nvgRGBA(8, 8, 8, 255));
-      nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
-      nvgFill(vg);
-
-      nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
-      nvgFontFace(vg, "opensans_semibold");
-      nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
-      nvgText(vg, b_x+b_w/2, b_y+b_h/2, button, NULL);
-
-      nvgBeginPath(vg);
-      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50));
-      nvgStrokeWidth(vg, 5);
-      nvgRoundedRect(vg, b_x, b_y, b_w, b_h, 20);
-      nvgStroke(vg);
-    }
-
-    // draw button
-    if (altbutton) {
-      nvgBeginPath(vg);
-      nvgFillColor(vg, nvgRGBA(8, 8, 8, 255));
-      nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
-      nvgFill(vg);
-
-      nvgFillColor(vg, nvgRGBA(255, 255, 255, 255));
-      nvgFontFace(vg, "opensans_semibold");
-      nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_MIDDLE);
-      nvgText(vg, balt_x+b_w/2, b_y+b_h/2, altbutton, NULL);
-
-      nvgBeginPath(vg);
-      nvgStrokeColor(vg, nvgRGBA(255, 255, 255, 50));
-      nvgStrokeWidth(vg, 5);
-      nvgRoundedRect(vg, balt_x, b_y, b_w, b_h, 20);
-      nvgStroke(vg);
-    }
-  }
-
-  void draw_battery_screen() {
-    low_battery_title = "Low Battery";
-    low_battery_text = "Please connect EON to your charger. Update will continue once EON battery reaches 35%.";
-    low_battery_context = "Current battery charge: " + battery_cap_text + "%";
-
-    nvgFillColor(vg, nvgRGBA(255,255,255,255));
-    nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
-
-    nvgFontFace(vg, "opensans_bold");
-    nvgFontSize(vg, 120.0f);
-    nvgTextBox(vg, 110, 220, fb_w-240, low_battery_title.c_str(), NULL);
-
-    nvgFontFace(vg, "opensans_regular");
-    nvgFontSize(vg, 86.0f);
-    nvgTextBox(vg, 130, 380, fb_w-260, low_battery_text.c_str(), NULL);
-
-    nvgFontFace(vg, "opensans_bold");
-    nvgFontSize(vg, 86.0f);
-    nvgTextBox(vg, 130, 700, fb_w-260, low_battery_context.c_str(), NULL);
-  }
-
-  void draw_progress_screen() {
-    // draw progress message
-    nvgFontSize(vg, 64.0f);
-    nvgFillColor(vg, nvgRGBA(255,255,255,255));
-    nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_BASELINE);
-    nvgFontFace(vg, "opensans_bold");
-    nvgFontSize(vg, 86.0f);
-    nvgTextBox(vg, 0, 380, fb_w, progress_text.c_str(), NULL);
-
-    // draw progress bar
-    {
-      int progress_width = 1000;
-      int progress_x = fb_w/2-progress_width/2;
-      int progress_y = 520;
-      int progress_height = 50;
-
-      int powerprompt_y = 312;
-      nvgFontFace(vg, "opensans_regular");
-      nvgFontSize(vg, 64.0f);
-      nvgText(vg, fb_w/2, 740, "Ensure your device remains connected to a power source.", NULL);
-
-      NVGpaint paint = nvgBoxGradient(
-          vg, progress_x + 1, progress_y + 1,
-          progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
-      nvgBeginPath(vg);
-      nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
-      nvgFillPaint(vg, paint);
-      nvgFill(vg);
-
-      float value = std::min(std::max(0.0f, progress_frac), 1.0f);
-      int bar_pos = ((progress_width - 2) * value);
-
-      paint = nvgBoxGradient(
-          vg, progress_x, progress_y,
-          bar_pos+1.5f, progress_height-1, 3, 4,
-          nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
-
-      nvgBeginPath(vg);
-      nvgRoundedRect(
-          vg, progress_x+1, progress_y+1,
-          bar_pos, progress_height-2, 12);
-      nvgFillPaint(vg, paint);
-      nvgFill(vg);
-    }
-  }
-
-  void ui_draw() {
-    std::lock_guard<std::mutex> guard(lock);
-
-    nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
-
-    switch (state) {
-    case CONFIRMATION:
-      draw_ack_screen("An update to NEOS is required.",
-                      "Your device will now be reset and upgraded. You may want to connect to wifi as download is around 1 GB. Existing data on device should not be lost.",
-                      "Continue",
-                      "Connect to WiFi");
-      break;
-    case LOW_BATTERY:
-      draw_battery_screen();
-      break;
-    case RUNNING:
-      draw_progress_screen();
-      break;
-    case ERROR:
-      draw_ack_screen("There was an error", (error_text).c_str(), NULL, "Reboot");
-      break;
-    }
-
-    nvgEndFrame(vg);
-  }
-
-  void ui_update() {
-    std::lock_guard<std::mutex> guard(lock);
-
-    if (state == ERROR || state == CONFIRMATION) {
-      int touch_x = -1, touch_y = -1;
-      int res = touch_poll(&touch, &touch_x, &touch_y, 0);
-      if (res == 1 && !is_settings_active()) {
-        if (touch_x >= b_x && touch_x < b_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) {
-          if (state == CONFIRMATION) {
-            state = RUNNING;
-            update_thread_handle = std::thread(&Updater::run_stages, this);
-          }
-        }
-        if (touch_x >= balt_x && touch_x < balt_x+b_w && touch_y >= b_y && touch_y < b_y+b_h) {
-          if (state == CONFIRMATION) {
-            start_settings_activity("Settings$WifiSettingsActivity");
-          } else if (state == ERROR) {
-            do_exit = 1;
-          }
-        }
-      }
-    }
-  }
-
-  void go() {
-    ui_init();
-
-    while (!do_exit) {
-      ui_update();
-
-      glClearColor(0.08, 0.08, 0.08, 1.0);
-      glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
-
-      // background
-      nvgBeginPath(vg);
-      NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
-        nvgRGBA(0, 0, 0, 0), nvgRGBA(0, 0, 0, 255));
-      nvgFillPaint(vg, bg);
-      nvgRect(vg, 0, 0, fb_w, fb_h);
-      nvgFill(vg);
-
-      glEnable(GL_BLEND);
-      glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-
-      ui_draw();
-
-      glDisable(GL_BLEND);
-
-      fb->swap();
-
-      assert(glGetError() == GL_NO_ERROR);
-
-      // no simple way to do 30fps vsync with surfaceflinger...
-      util::sleep_for(30);
-    }
-
-    if (update_thread_handle.joinable()) {
-      update_thread_handle.join();
-    }
-
-    // reboot
-    system("service call power 16 i32 0 i32 0 i32 1");
-  }
-
-};
-
-}
-
-int main(int argc, char *argv[]) {
-  bool background_cache = false;
-  if (argc > 1) {
-    if (strcmp(argv[1], "local") == 0) {
-      manifest_url = MANIFEST_URL_NEOS_LOCAL;
-    } else if (strcmp(argv[1], "staging") == 0) {
-      manifest_url = MANIFEST_URL_NEOS_STAGING;
-    } else if (strcmp(argv[1], "bgcache") == 0) {
-      manifest_url = argv[2];
-      background_cache = true;
-    } else {
-      manifest_url = argv[1];
-    }
-  }
-
-  printf("updating from %s\n", manifest_url);
-  Updater updater;
-
-  int err = 0;
-  if (background_cache) {
-    err = !updater.download_stage();
-  } else {
-    updater.go();
-  }
-  return err;
-}
diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh
index 730b383a5..7ef671c26 100755
--- a/launch_chffrplus.sh
+++ b/launch_chffrplus.sh
@@ -90,18 +90,11 @@ function two_init {
 
   # Check for NEOS update
   if [ $(< /VERSION) != "$REQUIRED_NEOS_VERSION" ]; then
-    if [ -f "$DIR/scripts/continue.sh" ]; then
-      cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh"
-    fi
-
-    if [ ! -f "$BASEDIR/prebuilt" ]; then
-      # Clean old build products, but preserve the scons cache
-      cd $DIR
-      git clean -xdf
-      git submodule foreach --recursive git clean -xdf
-    fi
-
-    "$DIR/installer/updater/updater" "file://$DIR/installer/updater/update.json"
+    echo "Installing NEOS update"
+    NEOS_PY="$DIR/selfdrive/hardware/eon/neos.py"
+    MANIFEST="$DIR/selfdrive/hardware/eon/neos.json"
+    $NEOS_PY --swap-if-ready $MANIFEST
+    $DIR/selfdrive/hardware/eon/updater $NEOS_PY $MANIFEST
   fi
 }
 
@@ -165,11 +158,6 @@ function launch {
           mv "${STAGING_ROOT}/finalized" $BASEDIR
           cd $BASEDIR
 
-          # Partial mitigation for symlink-related filesystem corruption
-          # Ensure all files match the repo versions after update
-          git reset --hard
-          git submodule foreach --recursive git reset --hard
-
           echo "Restarting launch script ${LAUNCHER_LOCATION}"
           unset REQUIRED_NEOS_VERSION
           unset AGNOS_VERSION
diff --git a/launch_env.sh b/launch_env.sh
index 69c989f23..47d98cc92 100755
--- a/launch_env.sh
+++ b/launch_env.sh
@@ -11,7 +11,7 @@ if [ -z "$REQUIRED_NEOS_VERSION" ]; then
 fi
 
 if [ -z "$AGNOS_VERSION" ]; then
-  export AGNOS_VERSION="1.5"
+  export AGNOS_VERSION="2"
 fi
 
 if [ -z "$PASSIVE" ]; then
diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc
index 941020e28..be61a6c30 100644
Binary files a/models/dmonitoring_model_q.dlc and b/models/dmonitoring_model_q.dlc differ
diff --git a/models/supercombo.dlc b/models/supercombo.dlc
index 2641f23e4..3a4657590 100644
Binary files a/models/supercombo.dlc and b/models/supercombo.dlc differ
diff --git a/opendbc/acura_rdx_2020_can_generated.dbc b/opendbc/acura_rdx_2020_can_generated.dbc
index f9720ce5a..a114e4e13 100644
--- a/opendbc/acura_rdx_2020_can_generated.dbc
+++ b/opendbc/acura_rdx_2020_can_generated.dbc
@@ -281,6 +281,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/can/dbc.py b/opendbc/can/dbc.py
index 32d8fffa4..043a674a3 100755
--- a/opendbc/can/dbc.py
+++ b/opendbc/can/dbc.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
 import re
 import os
 import struct
@@ -148,7 +149,7 @@ class dbc():
       ival = dd.get(s.name)
       if ival is not None:
 
-        ival = (ival / s.factor) - s.offset
+        ival = (ival - s.offset) / s.factor
         ival = int(round(ival))
 
         if s.is_signed and ival < 0:
@@ -254,23 +255,3 @@ class dbc():
   def get_signals(self, msg):
     msg = self.lookup_msg_id(msg)
     return [sgs.name for sgs in self.msgs[msg][1]]
-
-
-if __name__ == "__main__":
-   from opendbc import DBC_PATH
-
-   dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc'))
-   msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2})
-   encoded = dbc_test.encode(*msg)
-   decoded = dbc_test.decode((0x25, 0, encoded))
-   assert decoded == msg
-
-   dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc'))
-   decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12"))
-   assert abs(decoded[1]['SAS_Angle'] - (-26.2)) < 0.001
-
-   msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0})
-   encoded = dbc_test.encode(*msg)
-   decoded = dbc_test.decode((0x2b0, 0, encoded))
-
-   assert decoded == msg
diff --git a/opendbc/can/process_dbc.py b/opendbc/can/process_dbc.py
index 811e59b2f..04a67db95 100755
--- a/opendbc/can/process_dbc.py
+++ b/opendbc/can/process_dbc.py
@@ -54,7 +54,7 @@ def process(in_fn, out_fn):
     checksum_start_bit = 0
     counter_start_bit = None
     little_endian = True
-  elif can_dbc.name.startswith(("chrysler_")):
+  elif can_dbc.name.startswith(("chrysler_", "stellantis_")):
     checksum_type = "chrysler"
     checksum_size = 8
     counter_size = None
diff --git a/opendbc/gm_global_a_powertrain.dbc b/opendbc/gm_global_a_powertrain.dbc
index 2b830c9bb..cf34478d0 100644
--- a/opendbc/gm_global_a_powertrain.dbc
+++ b/opendbc/gm_global_a_powertrain.dbc
@@ -72,8 +72,13 @@ BO_ 201 ECMEngineStatus: 8 K20_ECM
  SG_ Brake_Pressed : 40|1@0+ (1,0) [0|1] "" NEO
  SG_ Standstill : 2|1@0+ (1,0) [0|1] "" NEO
 
-BO_ 209 EBCMBrakePedalTorque: 7 K17_EBCM
- SG_ BrakePedalTorque : 3|12@0+ (1,0) [0|0] ""  NEO
+BO_ 209 EBCMBrakePedalSensors: 7 K17_EBCM
+ SG_ Counter1 : 7|2@0+ (1,0) [0|3] "" XXX
+ SG_ Counter2 : 23|2@0+ (1,0) [0|3] "" XXX
+ SG_ BrakePedalPosition1 : 5|14@0+ (1,0) [0|16383] "" XXX
+ SG_ BrakePedalPosition2 : 21|14@0- (-1,0) [0|16383] "" XXX
+ SG_ BrakeNormalized1 : 39|8@0+ (1,0) [0|255] "" XXX
+ SG_ BrakeNormalized2 : 47|8@0- (-1,0) [0|255] "" XXX
 
 BO_ 241 EBCMBrakePedalPosition: 6 K17_EBCM
  SG_ BrakePedalPosition : 15|8@0+ (1,0) [0|255] ""  NEO
diff --git a/opendbc/honda_accord_2018_can_generated.dbc b/opendbc/honda_accord_2018_can_generated.dbc
index ff286c73d..1c78b38b6 100644
--- a/opendbc/honda_accord_2018_can_generated.dbc
+++ b/opendbc/honda_accord_2018_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
index c2bd5c52e..fe9a13af8 100644
--- a/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
+++ b/opendbc/honda_civic_hatchback_ex_2017_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
index 085ee1de3..8e7199cc9 100644
--- a/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
+++ b/opendbc/honda_civic_sedan_16_diesel_2019_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/honda_crv_ex_2017_can_generated.dbc b/opendbc/honda_crv_ex_2017_can_generated.dbc
index 2f1c85604..cef50babc 100644
--- a/opendbc/honda_crv_ex_2017_can_generated.dbc
+++ b/opendbc/honda_crv_ex_2017_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/honda_crv_hybrid_2019_can_generated.dbc b/opendbc/honda_crv_hybrid_2019_can_generated.dbc
index 99d3a8d1a..4a6ad4b82 100644
--- a/opendbc/honda_crv_hybrid_2019_can_generated.dbc
+++ b/opendbc/honda_crv_hybrid_2019_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/honda_insight_ex_2019_can_generated.dbc b/opendbc/honda_insight_ex_2019_can_generated.dbc
index 7a511751f..024f68d3c 100644
--- a/opendbc/honda_insight_ex_2019_can_generated.dbc
+++ b/opendbc/honda_insight_ex_2019_can_generated.dbc
@@ -285,6 +285,7 @@ BO_ 777 CAR_SPEED: 8 PCM
  SG_ LOCK_STATUS : 55|2@0+ (1,0) [0|255] "" XXX
  SG_ COUNTER : 61|2@0+ (1,0) [0|3] "" XXX
  SG_ CHECKSUM : 59|4@0+ (1,0) [0|15] "" XXX
+ SG_ IMPERIAL_UNIT : 63|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 780 ACC_HUD: 8 ADAS
  SG_ CRUISE_SPEED : 31|8@0+ (1,0) [0|255] "kph" BDY
diff --git a/opendbc/hyundai_kia_mando_front_radar.dbc b/opendbc/hyundai_kia_mando_front_radar.dbc
new file mode 100644
index 000000000..552cdddea
--- /dev/null
+++ b/opendbc/hyundai_kia_mando_front_radar.dbc
@@ -0,0 +1,454 @@
+
+VERSION ""
+
+
+NS_ :
+    NS_DESC_
+    CM_
+    BA_DEF_
+    BA_
+    VAL_
+    CAT_DEF_
+    CAT_
+    FILTER
+    BA_DEF_DEF_
+    EV_DATA_
+    ENVVAR_DATA_
+    SGTYPE_
+    SGTYPE_VAL_
+    BA_DEF_SGTYPE_
+    BA_SGTYPE_
+    SIG_TYPE_REF_
+    VAL_TABLE_
+    SIG_GROUP_
+    SIG_VALTYPE_
+    SIGTYPE_VALTYPE_
+    BO_TX_BU_
+    BA_DEF_REL_
+    BA_REL_
+    BA_DEF_DEF_REL_
+    BU_SG_REL_
+    BU_EV_REL_
+    BU_BO_REL_
+    SG_MUL_VAL_
+
+BS_:
+
+BU_: XXX
+
+
+BO_ 1280 RADAR_TRACK_500: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1281 RADAR_TRACK_501: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1282 RADAR_TRACK_502: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1283 RADAR_TRACK_503: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1284 RADAR_TRACK_504: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1285 RADAR_TRACK_505: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1286 RADAR_TRACK_506: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1287 RADAR_TRACK_507: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1288 RADAR_TRACK_508: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1289 RADAR_TRACK_509: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1290 RADAR_TRACK_50a: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1291 RADAR_TRACK_50b: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1292 RADAR_TRACK_50c: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1293 RADAR_TRACK_50d: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1294 RADAR_TRACK_50e: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1295 RADAR_TRACK_50f: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1296 RADAR_TRACK_510: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1297 RADAR_TRACK_511: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1298 RADAR_TRACK_512: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1299 RADAR_TRACK_513: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1300 RADAR_TRACK_514: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1301 RADAR_TRACK_515: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1302 RADAR_TRACK_516: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1303 RADAR_TRACK_517: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1304 RADAR_TRACK_518: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1305 RADAR_TRACK_519: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1306 RADAR_TRACK_51a: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1307 RADAR_TRACK_51b: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1308 RADAR_TRACK_51c: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1309 RADAR_TRACK_51d: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1310 RADAR_TRACK_51e: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
+
+BO_ 1311 RADAR_TRACK_51f: 8 RADAR
+ SG_ UNKNOWN_1 : 7|8@0- (1,0) [-128|127] "" XXX
+ SG_ AZIMUTH : 12|10@0- (0.2,0) [-102.4|102.2] "" XXX
+ SG_ STATE : 15|3@0+ (1,0) [0|7] "" XXX
+ SG_ LONG_DIST : 18|11@0+ (0.1,0) [0|204.7] "" XXX
+ SG_ REL_ACCEL : 33|10@0- (0.02,0) [-10.24|10.22] "" XXX
+ SG_ ZEROS : 37|4@0+ (1,0) [0|255] "" XXX
+ SG_ COUNTER : 38|1@0+ (1,0) [0|1] "" XXX
+ SG_ STATE_3 : 39|1@0+ (1,0) [0|1] "" XXX
+ SG_ REL_SPEED : 53|14@0- (0.01,0) [-81.92|81.92] "" XXX
+ SG_ STATE_2 : 55|2@0+ (1,0) [0|3] "" XXX
+
diff --git a/opendbc/lexus_ct200h_2018_pt_generated.dbc b/opendbc/lexus_ct200h_2018_pt_generated.dbc
index 818ab6f3f..3b225c73e 100644
--- a/opendbc/lexus_ct200h_2018_pt_generated.dbc
+++ b/opendbc/lexus_ct200h_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_ct200h_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/lexus_is_2018_pt_generated.dbc b/opendbc/lexus_is_2018_pt_generated.dbc
index 711944283..51c584df6 100644
--- a/opendbc/lexus_is_2018_pt_generated.dbc
+++ b/opendbc/lexus_is_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_is_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/lexus_nx300_2018_pt_generated.dbc b/opendbc/lexus_nx300_2018_pt_generated.dbc
index 705d87d37..f00cd2c65 100644
--- a/opendbc/lexus_nx300_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_nx300_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/lexus_nx300h_2018_pt_generated.dbc b/opendbc/lexus_nx300h_2018_pt_generated.dbc
index f4bfb86cb..ddb4ad6de 100644
--- a/opendbc/lexus_nx300h_2018_pt_generated.dbc
+++ b/opendbc/lexus_nx300h_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_nx300h_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/lexus_rx_350_2016_pt_generated.dbc b/opendbc/lexus_rx_350_2016_pt_generated.dbc
index 6144153cd..4c316fee7 100644
--- a/opendbc/lexus_rx_350_2016_pt_generated.dbc
+++ b/opendbc/lexus_rx_350_2016_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_rx_350_2016_pt.dbc starts here";
 
 
diff --git a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
index 92437357c..c1d67f5c9 100644
--- a/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
+++ b/opendbc/lexus_rx_hybrid_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "lexus_rx_hybrid_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/mazda_2017.dbc b/opendbc/mazda_2017.dbc
index 9ee492928..fdd6d2824 100644
--- a/opendbc/mazda_2017.dbc
+++ b/opendbc/mazda_2017.dbc
@@ -498,32 +498,17 @@ BO_ 552 GEAR: 8 XXX
  SG_ GEAR_BOX : 36|4@0+ (1,0) [0|15] "" XXX
 
 BO_ 543 CRZ_EVENTS: 8 XXX
- SG_ NEW_SIGNAL_3 : 34|1@0+ (1,0) [0|1] "" XXX
- SG_ CHKSUM : 63|8@0+ (1,0) [0|15] "" XXX
- SG_ NEW_SIGNAL_1 : 55|4@0+ (1,0) [0|15] "" XXX
- SG_ NEW_SIGNAL_5 : 47|4@0+ (1,0) [0|15] "" XXX
- SG_ NEW_SIGNAL_6 : 6|1@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_8 : 0|1@0+ (1,0) [0|31] "" XXX
- SG_ NEW_SIGNAL_9 : 1|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_10 : 2|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_12 : 10|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_13 : 9|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_18 : 12|1@0+ (1,0) [0|15] "" XXX
- SG_ NEW_SIGNAL_19 : 14|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_20 : 15|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_21 : 23|1@0+ (1,0) [0|15] "" XXX
- SG_ NEW_SIGNAL_24 : 31|1@0+ (1,0) [0|1] "" XXX
- SG_ GAS_PEDAL_PRESSED : 32|1@0+ (1,0) [0|255] "" XXX
- SG_ CRZ_STARTED : 18|1@0+ (1,0) [0|1] "" XXX
- SG_ PLUS_ONE_CRZ : 17|1@0+ (1,0) [0|255] "" XXX
- SG_ PLUS_ONE_CRZ_2 : 19|1@0+ (1,0) [0|1] "" XXX
- SG_ GAS_MAYBE : 22|1@0+ (1,0) [0|1] "" XXX
- SG_ NONACC_ACTIVE : 5|1@0+ (1,0) [0|1] "" XXX
- SG_ CRUISE_ACTIVE_CAR_MOVING : 16|1@0+ (1,0) [0|1] "" XXX
- SG_ NONACC_RELATED : 11|1@0+ (1,0) [0|255] "" XXX
  SG_ CTR : 51|4@0+ (1,0) [0|15] "" XXX
- SG_ CAS_CMD_MAYBE : 30|7@0- (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_4 : 43|4@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_21 : 23|1@0+ (1,0) [0|15] "" XXX
+ SG_ GAS_MAYBE : 22|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACCEL_CMD : 46|8@0- (1,0) [0|15] "" XXX
+ SG_ CRUISE_ACTIVE_CAR_MOVING : 16|1@0+ (1,0) [0|1] "" XXX
+ SG_ PLUS_ONE_CRZ : 17|1@0+ (1,0) [0|255] "" XXX
+ SG_ CRZ_STARTED : 18|1@0+ (1,0) [0|1] "" XXX
+ SG_ CHKSUM : 63|8@0+ (1,0) [0|15] "" XXX
+ SG_ PLUS_ONE_CRZ_2 : 19|1@0+ (1,0) [0|1] "" XXX
+ SG_ ACCEL_CMD_LOW_RES : 29|8@0- (1,0) [0|255] "" XXX
+ SG_ CRZ_SPEED : 7|16@0+ (0.005,-0.5) [0|328] "kph" XXX
 
 BO_ 542 NEW_MSG_33: 8 XXX
  SG_ CTR : 48|4@1+ (1,0) [0|15] "" XXX
@@ -564,32 +549,32 @@ BO_ 535 CURVE_CTRS: 8 XXX
  SG_ NEW_SIGNAL_4 : 9|1@0+ (1,0) [0|1] "" XXX
 
 BO_ 540 CRZ_CTRL: 8 XXX
- SG_ NEW_SIGNAL_3 : 1|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_4 : 8|1@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_5 : 9|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_6 : 10|1@0+ (1,0) [0|1] "" XXX
- SG_ NEW_SIGNAL_7 : 18|3@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_9 : 31|1@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_10 : 30|1@0+ (1,0) [0|1] "" XXX
  SG_ ACC_GAS_MAYBE : 23|1@0+ (1,0) [0|31] "" XXX
  SG_ ACC_GAS_MAYBE2 : 29|1@0+ (1,0) [0|1] "" XXX
- SG_ CRZ_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX
  SG_ HANDS_OFF_STEERING : 48|1@0+ (1,0) [0|1] "" XXX
  SG_ HANDS_ON_STEER_WARN : 59|4@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_1 : 52|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_2 : 45|3@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_8 : 0|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_3 : 1|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRZ_ACTIVE : 3|1@0+ (1,0) [0|1] "" XXX
+ SG_ NEW_SIGNAL_4 : 8|1@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_5 : 9|1@0+ (1,0) [0|1] "" XXX
+ SG_ CRZ_AVAILABLE : 17|1@0+ (1,0) [0|255] "" XXX
+ SG_ DISTANCE_SETTING : 20|3@0+ (1,0) [0|7] "" XXX
 
 BO_ 539 CRZ_INFO: 8 XXX
- SG_ NEW_SIGNAL_1 : 17|1@0+ (1,0) [0|255] "" XXX
- SG_ NEW_SIGNAL_2 : 16|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_5 : 34|1@0+ (1,0) [0|1] "" XXX
  SG_ NEW_SIGNAL_7 : 47|1@0+ (1,0) [0|255] "" XXX
  SG_ CTR1 : 55|8@0+ (1,0) [0|255] "" XXX
  SG_ NEW_SIGNAL_3 : 37|1@0+ (1,0) [0|255] "" XXX
  SG_ ACC_ACTIVE : 33|1@0+ (1,0) [0|1] "" XXX
- SG_ ACCEL_CMD : 31|10@0- (1,0) [0|1] "" XXX
+ SG_ ACCEL_CMD : 31|8@0- (1,0) [0|1] "" XXX
  SG_ CHKSUM : 63|8@0+ (1,0) [0|255] "" XXX
+ SG_ NEW_SIGNAL_1 : 7|16@0+ (1,0) [0|65535] "" XXX
 
 BO_ 121 EPB: 8 XXX
  SG_ NEW_SIGNAL_1 : 4|4@0+ (1,0) [0|255] "" XXX
@@ -774,5 +759,6 @@ CM_ SG_ 552 MORE_GEAR "";
 CM_ SG_ 552 GEAR "0 Shifting, 1 P, 2 R, 3 N, 4 D";
 CM_ SG_ 552 GEAR_BOX "0 P, 14 R, 1 though 6 D for speeds, 15 Shift";
 CM_ SG_ 540 HANDS_ON_STEER_WARN "0 no warning, b warning";
+CM_ SG_ 540 DISTANCE_SETTING "Radar distance 0: disabled, 1: 4 bars, 2: 3 bars, 3: 2 bars, 4: 1 bar";
 CM_ SG_ 1143 REAR_CT_ALERT "Rear Cross Traffic Alert";
 VAL_ 552 GEAR 1 "P" 2 "R" 3 "N" 4 "D" ;
diff --git a/opendbc/tesla_can.dbc b/opendbc/tesla_can.dbc
index cdc665dc7..57a63e128 100644
--- a/opendbc/tesla_can.dbc
+++ b/opendbc/tesla_can.dbc
@@ -34,14 +34,14 @@ NS_ :
 BS_:
 
 BU_:
-    NEO
-    MCU
-    GTW
-    EPAS
-    DI
-    ESP
-    SBW
-    STW
+	NEO
+	MCU
+	GTW
+	EPAS
+	DI
+	ESP
+	SBW
+	STW
 	APP
 	DAS
 	XXX
@@ -71,6 +71,17 @@ BO_ 1160 DAS_steeringControl: 4 NEO
  SG_ DAS_steeringAngleRequest : 6|15@0+ (0.1,-1638.35) [-1638.35|1638.35] "deg" EPAS
  SG_ DAS_steeringHapticRequest : 7|1@0+ (1,0) [0|0] "" EPAS
 
+BO_ 697 DAS_control: 8 NEO
+ SG_ DAS_setSpeed : 0|12@1+ (0.1,0) [0|409.4] "kph" NEO
+ SG_ DAS_accState : 12|4@1+ (1,0) [0|0] "" NEO
+ SG_ DAS_aebEvent : 16|2@1+ (1,0) [0|3] "" NEO
+ SG_ DAS_jerkMin : 18|9@1+ (0.03,-15.232) [-15.232|0.098] "m/s^3" NEO
+ SG_ DAS_jerkMax : 27|8@1+ (0.059,0) [0|15.045] "m/s^3" NEO
+ SG_ DAS_accelMin : 35|9@1+ (0.04,-15) [-15|5.44] "m/s^2" NEO
+ SG_ DAS_accelMax : 44|9@1+ (0.04,-15) [-15|5.44] "m/s^2" NEO
+ SG_ DAS_controlCounter : 53|3@1+ (1,0) [0|0] "" NEO
+ SG_ DAS_controlChecksum : 56|8@1+ (1,0) [0|0] "" NEO
+
 BO_ 257 GTW_epasControl: 3 NEO
  SG_ GTW_epasControlChecksum : 23|8@0+ (1,0) [0|255] ""  NEO
  SG_ GTW_epasControlCounter : 11|4@0+ (1,0) [0|15] ""  NEO
@@ -102,7 +113,6 @@ BO_ 3 STW_ANGL_STAT: 8 STW
  SG_ CRC_STW_ANGL_STAT : 63|8@0+ (1,0) [0|255] ""  NEO
 
 BO_ 14 STW_ANGLHP_STAT: 8 STW
-
  SG_ StW_AnglHP : 5|14@0+ (0.1,-819.2) [-819.2|819] "deg"  NEO
  SG_ StW_AnglHP_Spd : 21|14@0+ (0.5,-4096) [-4096|4095.5] "deg/s"  NEO
  SG_ StW_AnglHP_Sens_Stat : 33|2@0+ (1,0) [0|0] ""  NEO
@@ -691,6 +701,13 @@ VAL_ 643 MPkBrk_Stat 1 "ENGG" 0 "RELS" ;
 VAL_ 643 RevGr_Engg 0 "DISENGG" 1 "ENGG" 2 "NDEF2" 3 "SNA" ;
 VAL_ 643 StW_Cond_Stat 3 "BLINK" 1 "NDEF1" 0 "OFF" 2 "ON" ;
 VAL_ 643 Trlr_Stat 2 "NDEF2" 0 "NONE" 1 "OK" 3 "SNA" ;
+VAL_ 697 DAS_setSpeed 4095 "SNA" ;
+VAL_ 697 DAS_accState 15 "FAULT_SNA" 13 "ACC_CANCEL_GENERIC_SILENT" 11 "APC_SELFPARK_START" 10 "APC_UNPARK_COMPLETE" 9 "APC_PAUSE" 8 "APC_ABORT" 7 "APC_COMPLETE" 6 "APC_FORWARD" 5 "APC_BACKWARD" 4 "ACC_ON" 3 "ACC_HOLD" 0 "ACC_CANCEL_GENERIC" ;
+VAL_ 697 DAS_aebEvent 3 "AEB_SNA" 2 "AEB_FAULT" 1 "AEB_ACTIVE" 0 "AEB_NOT_ACTIVE" ;
+VAL_ 697 DAS_jerkMin 511 "SNA" ;
+VAL_ 697 DAS_jerkMax 255 "SNA" ;
+VAL_ 697 DAS_accelMin 511 "SNA" ;
+VAL_ 697 DAS_accelMax 511 "SNA" ;
 VAL_ 792 BOOT_STATE 2 "Init" 3 "SNA" 0 "closed" 1 "open" ;
 VAL_ 792 CERRD 1 "CAN error detect" 0 "no Can error detected" ;
 VAL_ 792 DAY 1 "Init" 0 "SNA" ;
diff --git a/opendbc/toyota_avalon_2017_pt_generated.dbc b/opendbc/toyota_avalon_2017_pt_generated.dbc
index b7da0d4bd..25753613d 100644
--- a/opendbc/toyota_avalon_2017_pt_generated.dbc
+++ b/opendbc/toyota_avalon_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_avalon_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
index 4798539c4..ec35feef2 100644
--- a/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_camry_hybrid_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_camry_hybrid_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_corolla_2017_pt_generated.dbc b/opendbc/toyota_corolla_2017_pt_generated.dbc
index bea949426..db13a143a 100644
--- a/opendbc/toyota_corolla_2017_pt_generated.dbc
+++ b/opendbc/toyota_corolla_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_corolla_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_highlander_2017_pt_generated.dbc b/opendbc/toyota_highlander_2017_pt_generated.dbc
index 878ec9a74..ca7c16b1e 100644
--- a/opendbc/toyota_highlander_2017_pt_generated.dbc
+++ b/opendbc/toyota_highlander_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_highlander_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
index 38368bac6..843d1e2ce 100644
--- a/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
+++ b/opendbc/toyota_highlander_hybrid_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_highlander_hybrid_2018_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
index 64b04707b..0f94c584c 100644
--- a/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_hybrid_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 
 CM_ "Imported file _toyota_nodsu_common.dbc starts here";
 BO_ 401 STEERING_LTA: 8 XXX
diff --git a/opendbc/toyota_nodsu_pt_generated.dbc b/opendbc/toyota_nodsu_pt_generated.dbc
index 057546027..10cf508b7 100644
--- a/opendbc/toyota_nodsu_pt_generated.dbc
+++ b/opendbc/toyota_nodsu_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 
 CM_ "Imported file _toyota_nodsu_common.dbc starts here";
 BO_ 401 STEERING_LTA: 8 XXX
diff --git a/opendbc/toyota_prius_2017_pt_generated.dbc b/opendbc/toyota_prius_2017_pt_generated.dbc
index 59bbf368a..1d29dbf81 100644
--- a/opendbc/toyota_prius_2017_pt_generated.dbc
+++ b/opendbc/toyota_prius_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_prius_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_rav4_2017_pt_generated.dbc b/opendbc/toyota_rav4_2017_pt_generated.dbc
index 5bb60ce7e..7a2f0bd4c 100644
--- a/opendbc/toyota_rav4_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_rav4_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
index 966984f65..61ff80a6d 100644
--- a/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
+++ b/opendbc/toyota_rav4_hybrid_2017_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_rav4_hybrid_2017_pt.dbc starts here";
 
 
diff --git a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
index 883a45b99..ac7660643 100644
--- a/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
+++ b/opendbc/toyota_sienna_xle_2018_pt_generated.dbc
@@ -165,10 +165,12 @@ BO_ 1020 SOLAR_SENSOR: 8 XXX
  SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
 
 BO_ 1041 ACC_HUD: 8 DSU
+ SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
  SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
  SG_ SET_ME_X10 : 39|8@0+ (1,0) [0|1] "" XXX
- SG_ SET_ME_X80 : 55|8@0+ (1,0) [0|1] "" XXX
+ SG_ PCS_OFF : 40|1@0+ (1,0) [0|0] "" XXX
+ SG_ PCS_SENSITIVITY : 55|8@0+ (1,0) [0|1] "" XXX
 
 BO_ 1042 LKAS_HUD: 8 XXX
  SG_ BARRIERS : 1|2@0+ (1,0) [0|3] "" XXX
@@ -313,6 +315,8 @@ CM_ SG_ 835 PERMIT_BRAKING "Original ACC has this going high when a car in front
 CM_ SG_ 835 ACCEL_CMD_ALT "Copy of main ACCEL_CMD, but across 8 bits instead of 16 bits like ACCEL_CMD. Unsure if only informational or has an effect. Likely informational as existing openpilot sets this to 0 and no loss of functionality observed. Originally 'AT_RAW' in leaked toyota_2017_ref_pt.dbc file.";
 CM_ SG_ 921 UI_SET_SPEED "set speed shown in UI with user set unit";
 CM_ SG_ 951 BRAKE_LIGHTS_ACC "brake lights when ACC commands decel";
+CM_ SG_ 1041 PCS_INDICATOR "Pre-Collision System Indicator";
+CM_ SG_ 1041 PCS_SENSITIVITY "Pre-Collision System Sensitivity";
 CM_ SG_ 1042 SET_ME_1 "unclear what this is, but it's always 1 in drive traces";
 CM_ SG_ 1042 REPEATED_BEEPS "recommended for fcw and other important alerts";
 CM_ SG_ 1161 SPDVAL1 "Numbers 0-199 is displayed, 200-254 displays circle without number and 255 is for no limit.";
@@ -338,6 +342,8 @@ VAL_ 614 DIRECTION_CMD 3 "right" 2 "center" 1 "left";
 VAL_ 643 STATE 0 "normal" 1 "adaptive_cruise_control" 3 "emergency_braking";
 VAL_ 835 ACC_TYPE 2 "permanent low speed lockout" 1 "ok";
 VAL_ 921 CRUISE_CONTROL_STATE 2 "disabled" 11 "hold" 10 "hold_waiting_user_cmd" 6 "enabled" 5 "faulted";
+VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
+VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity"  128 "mid sensitivity" 192 "low sensitivity" 0 "off";
 VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
 VAL_ 1042 BARRIERS 3 "both" 2 "right" 1 "left" 0 "none";
 VAL_ 1042 RIGHT_LINE 3 "orange" 2 "faded" 1 "solid" 0 "none";
@@ -388,6 +394,13 @@ BO_ 35 SECONDARY_STEER_ANGLE: 8 XXX
 
 CM_ "BO_ SECONDARY_STEER_ANGLE: ZSS is a high-precision steering angle sensor that can replace the lower resolution sensor in most TSS1 Toyotas. Learn more: https://github.com/commaai/openpilot/wiki/Toyota-Lexus#zorro-steering-sensor-zss";
 
+BO_ 767 SDSU: 8 XXX
+ SG_ FD_BUTTON : 7|1@0+ (1,0) [0|1] "" XXX
+
+CM_ "BO_ SDSU: The sDSU is a modified DSU for use in TSS1 Toyotas. Learn more: https://github.com/wocsor/panda/tree/smart_dsu";
+
+CM_ SG_ 767 FD_BUTTON "The follow distance button signal as forwarded by the sdsu";
+
 CM_ "toyota_sienna_xle_2018_pt.dbc starts here";
 
 
diff --git a/panda/board/SConscript b/panda/board/SConscript
index c16d191bd..4570bf1a5 100644
--- a/panda/board/SConscript
+++ b/panda/board/SConscript
@@ -4,49 +4,35 @@ import subprocess
 PREFIX = "arm-none-eabi-"
 BUILDER = "DEV"
 
+common_flags = []
+build_projects = {}
+
 if os.getenv("PEDAL"):
-  PROJECT = "pedal"
-  STARTUP_FILE = "stm32fx/startup_stm32f205xx.s"
-  LINKER_SCRIPT = "stm32fx/stm32fx_flash.ld"
-  APP_START_ADDRESS = "0x8004000"
-  MAIN = "pedal/main.c"
-  PROJECT_FLAGS = [
-    "-mcpu=cortex-m3",
-    "-msoft-float",
-    "-DSTM32F2",
-    "-DSTM32F205xx",
-    "-O2",
-    "-DPEDAL",
-  ]
+  build_projects["pedal"] = {
+    "MAIN": "pedal/main.c",
+    "STARTUP_FILE": "stm32fx/startup_stm32f205xx.s",
+    "LINKER_SCRIPT": "stm32fx/stm32fx_flash.ld",
+    "APP_START_ADDRESS": "0x8004000",
+    "PROJECT_FLAGS": [
+      "-mcpu=cortex-m3",
+      "-msoft-float",
+      "-DSTM32F2",
+      "-DSTM32F205xx",
+      "-O2",
+      "-DPEDAL",
+    ],
+  }
+
   if os.getenv("PEDAL_USB"):
-    PROJECT = "pedal_usb"
-    PROJECT_FLAGS.append("-DPEDAL_USB")
+    build_projects["pedal_usb"] = build_projects.pop("pedal")
+    build_projects["pedal_usb"]["PROJECT_FLAGS"].append("-DPEDAL_USB")
 
-elif os.getenv("PANDA_H7"):
-  PROJECT = "panda_h7"
-  STARTUP_FILE = "stm32h7/startup_stm32h7x5xx.s"
-  LINKER_SCRIPT = "stm32h7/stm32h7x5_flash.ld"
-  APP_START_ADDRESS = "0x8020000"
-  MAIN = "main.c"
-  PROJECT_FLAGS = [
-    "-mcpu=cortex-m7",
-    "-mhard-float",
-    "-DSTM32H7",
-    "-DSTM32H725xx",
-    "-mfpu=fpv5-d16",
-    "-fsingle-precision-constant",
-    "-Os",
-    "-g",
-    "-DPANDA",
-  ]
-
-else:
-  PROJECT = "panda"
-  STARTUP_FILE = "stm32fx/startup_stm32f413xx.s"
-  LINKER_SCRIPT = "stm32fx/stm32fx_flash.ld"
-  APP_START_ADDRESS = "0x8004000"
-  MAIN = "main.c"
-  PROJECT_FLAGS = [
+build_projects["panda"] = {
+  "MAIN": "main.c",
+  "STARTUP_FILE": "stm32fx/startup_stm32f413xx.s",
+  "LINKER_SCRIPT": "stm32fx/stm32fx_flash.ld",
+  "APP_START_ADDRESS": "0x8004000",
+  "PROJECT_FLAGS": [
     "-mcpu=cortex-m4",
     "-mhard-float",
     "-DSTM32F4",
@@ -56,8 +42,48 @@ else:
     "-Os",
     "-g",
     "-DPANDA",
-  ]
+  ],
+}
 
+# arm-none-eabi-gcc on comma two has no cortex-m7 support
+if not os.path.exists("/EON"):
+  build_projects["panda_h7"] = {
+    "MAIN": "main.c",
+    "STARTUP_FILE": "stm32h7/startup_stm32h7x5xx.s",
+    "LINKER_SCRIPT": "stm32h7/stm32h7x5_flash.ld",
+    "APP_START_ADDRESS": "0x8020000",
+    "PROJECT_FLAGS": [
+      "-mcpu=cortex-m7",
+      "-mhard-float",
+      "-DSTM32H7",
+      "-DSTM32H725xx",
+      "-mfpu=fpv5-d16",
+      "-fsingle-precision-constant",
+      "-Os",
+      "-g",
+      "-DPANDA",
+    ],
+  }
+
+if os.getenv("RELEASE"):
+  BUILD_TYPE = "RELEASE"
+  cert_fn = os.getenv("CERT")
+  assert cert_fn is not None, 'No certificate file specified. Please set CERT env variable'
+  assert os.path.exists(cert_fn), 'Certificate file not found. Please specify absolute path'
+else:
+  BUILD_TYPE = "DEBUG"
+  cert_fn = File("../certs/debug").srcnode().abspath
+  common_flags += ["-DALLOW_DEBUG"]
+
+if os.getenv("DEBUG"):
+  common_flags += ["-DDEBUG"]
+
+includes = [
+  "stm32fx/inc",
+  "stm32h7/inc",
+  "..",
+  ".",
+]
 
 def get_version(builder, build_type):
   try:
@@ -96,83 +122,67 @@ def get_key_header(name):
   ]
   return r
 
-
 def objcopy(source, target, env, for_signature):
     return '$OBJCOPY -O binary %s %s' % (source[0], target[0])
 
-
-linkerscript_fn = File(LINKER_SCRIPT).srcnode().abspath
-
-flags = [
-  "-Wall",
-  "-Wextra",
-  "-Wstrict-prototypes",
-  "-Werror",
-  "-mlittle-endian",
-  "-mthumb",
-  "-nostdlib",
-  "-fno-builtin",
-  f"-T{linkerscript_fn}",
-  "-std=gnu11",
-] + PROJECT_FLAGS
-
-
-if os.getenv("RELEASE"):
-  BUILD_TYPE = "RELEASE"
-  cert_fn = os.getenv("CERT")
-  assert cert_fn is not None, 'No certificate file specified. Please set CERT env variable'
-  assert os.path.exists(cert_fn), 'Certificate file not found. Please specify absolute path'
-else:
-  BUILD_TYPE = "DEBUG"
-  cert_fn = File("../certs/debug").srcnode().abspath
-  flags += ["-DALLOW_DEBUG"]
-
-includes = [
-  "stm32fx/inc",
-  "stm32h7/inc",
-  "..",
-  ".",
-]
-
-panda_env = Environment(
-  ENV=os.environ,
-  CC=PREFIX + 'gcc',
-  AS=PREFIX + 'gcc',
-  OBJCOPY=PREFIX + 'objcopy',
-  OBJDUMP=PREFIX + 'objdump',
-  ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES",
-  CFLAGS=flags,
-  ASFLAGS=flags,
-  LINKFLAGS=flags,
-  CPPPATH=includes,
-  BUILDERS={
-    'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf')
-  }
-)
-
 # Common autogenerated includes
-version = f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";'
-gitversion = panda_env.Textfile("obj/gitversion.h", [version, ""])
-Ignore('bootstub.o', gitversion)
-Requires('bootstub.o', gitversion)
-Ignore('main.o', gitversion)
-Requires('main.o', gitversion)
+with open("obj/gitversion.h", "w") as f:
+  f.write(f'const uint8_t gitversion[] = "{get_version(BUILDER, BUILD_TYPE)}";\n')
 
 certs = [get_key_header(n) for n in ["debug", "release"]]
-certheader = panda_env.Textfile("obj/cert.h", certs + [""])
+with open("obj/cert.h", "w") as f:
+  for cert in certs:
+    f.write("\n".join(cert) + "\n")
 
-startup = panda_env.Object(STARTUP_FILE)
+for project_name in build_projects:
+  project = build_projects[project_name]
+  linkerscript_fn = File(project["LINKER_SCRIPT"]).srcnode().abspath
 
-# Bootstub
-crypto = ["../crypto/rsa.c", "../crypto/sha.c"]
-bootstub_elf = panda_env.Program(f"obj/bootstub.{PROJECT}.elf", [startup] + crypto + ["bootstub.c"])
-bootstub_bin = panda_env.Objcopy(f"obj/bootstub.{PROJECT}.bin", bootstub_elf)
+  flags = [
+    "-Wall",
+    "-Wextra",
+    "-Wstrict-prototypes",
+    "-Werror",
+    "-mlittle-endian",
+    "-mthumb",
+    "-nostdlib",
+    "-fno-builtin",
+    f"-T{linkerscript_fn}",
+    "-std=gnu11",
+  ] + project["PROJECT_FLAGS"] + common_flags
 
-# Build main
-main_elf = panda_env.Program(f"obj/{PROJECT}.elf", [startup, MAIN],
-  LINKFLAGS=[f"-Wl,--section-start,.isr_vector={APP_START_ADDRESS}"] + flags)
-main_bin = panda_env.Objcopy(f"obj/{PROJECT}.bin", main_elf)
+  project_env = Environment(
+    ENV=os.environ,
+    CC=PREFIX + 'gcc',
+    AS=PREFIX + 'gcc',
+    OBJCOPY=PREFIX + 'objcopy',
+    OBJDUMP=PREFIX + 'objdump',
+    ASCOM="$AS $ASFLAGS -o $TARGET -c $SOURCES",
+    CFLAGS=flags,
+    ASFLAGS=flags,
+    LINKFLAGS=flags,
+    CPPPATH=includes,
+    BUILDERS={
+      'Objcopy': Builder(generator=objcopy, suffix='.bin', src_suffix='.elf')
+    }
+  )
+  startup = project_env.Object(project["STARTUP_FILE"])
 
-# Sign main
-sign_py = File("../crypto/sign.py").srcnode().abspath
-panda_bin_signed = panda_env.Command(f"obj/{PROJECT}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}")
+  # Bootstub
+  crypto_obj = [
+    project_env.Object(f"rsa-{project_name}", "../crypto/rsa.c"),
+    project_env.Object(f"sha-{project_name}", "../crypto/sha.c")
+  ]
+  bootstub_obj = project_env.Object(f"bootstub-{project_name}", "bootstub.c")
+  bootstub_elf = project_env.Program(f"obj/bootstub.{project_name}.elf", [startup] + crypto_obj + [bootstub_obj])
+  bootstub_bin = project_env.Objcopy(f"obj/bootstub.{project_name}.bin", bootstub_elf)
+
+  # Build main
+  main_obj = project_env.Object(f"main-{project_name}", project["MAIN"])
+  main_elf = project_env.Program(f"obj/{project_name}.elf", [startup, main_obj],
+    LINKFLAGS=[f"-Wl,--section-start,.isr_vector={project['APP_START_ADDRESS']}"] + flags)
+  main_bin = project_env.Objcopy(f"obj/{project_name}.bin", main_elf)
+
+  # Sign main
+  sign_py = File("../crypto/sign.py").srcnode().abspath
+  panda_bin_signed = project_env.Command(f"obj/{project_name}.bin.signed", main_bin, f"SETLEN=1 {sign_py} $SOURCE $TARGET {cert_fn}")
diff --git a/panda/board/bootstub_declarations.h b/panda/board/bootstub_declarations.h
index b247458f9..8e9f07312 100644
--- a/panda/board/bootstub_declarations.h
+++ b/panda/board/bootstub_declarations.h
@@ -2,6 +2,7 @@
 void puts(const char *a){ UNUSED(a); }
 void puth(uint8_t i){ UNUSED(i); }
 void puth2(uint8_t i){ UNUSED(i); }
+void puth4(uint8_t i){ UNUSED(i); }
 typedef struct board board;
 typedef struct harness_configuration harness_configuration;
 // No CAN support on bootloader
diff --git a/panda/board/build_all.sh b/panda/board/build_all.sh
index b7e681108..a461f6fa7 100755
--- a/panda/board/build_all.sh
+++ b/panda/board/build_all.sh
@@ -2,6 +2,5 @@
 set -e
 
 scons -u
-PANDA_H7=1 scons -u
 PEDAL=1 scons -u
 PEDAL=1 PEDAL_USB=1 scons -u
diff --git a/panda/board/drivers/bxcan.h b/panda/board/drivers/bxcan.h
index d631f0c72..7828925bd 100644
--- a/panda/board/drivers/bxcan.h
+++ b/panda/board/drivers/bxcan.h
@@ -141,9 +141,7 @@ void process_can(uint8_t can_number) {
         CAN->sTxMailBox[0].TDTR = to_send.RDTR;
         CAN->sTxMailBox[0].TIR = to_send.RIR;
 
-        if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
-          usb_outep3_resume_if_paused();
-        }
+        usb_cb_ep3_out_complete();
       }
     }
 
diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h
index 73d37aa84..cbce796d8 100644
--- a/panda/board/drivers/can_common.h
+++ b/panda/board/drivers/can_common.h
@@ -100,7 +100,21 @@ bool can_push(can_ring *q, CAN_FIFOMailBox_TypeDef *elem) {
   if (!ret) {
     can_overflow_cnt++;
     #ifdef DEBUG
-      puts("can_push failed!\n");
+      puts("can_push to ");
+      if (q == &can_rx_q) {
+        puts("can_rx_q");
+      } else if (q == &can_tx1_q) {
+        puts("can_tx1_q");
+      } else if (q == &can_tx2_q) {
+        puts("can_tx2_q");
+      } else if (q == &can_tx3_q) {
+        puts("can_tx3_q");
+      } else if (q == &can_txgmlan_q) {
+        puts("can_txgmlan_q");
+      } else {
+        puts("unknown");
+      }
+      puts(" failed!\n");
     #endif
   }
   return ret;
@@ -125,6 +139,8 @@ void can_clear(can_ring *q) {
   q->w_ptr = 0;
   q->r_ptr = 0;
   EXIT_CRITICAL();
+  // handle TX buffer full with zero ECUs awake on the bus
+  usb_cb_ep3_out_complete();
 }
 
 // assign CAN numbering
diff --git a/panda/board/drivers/fdcan.h b/panda/board/drivers/fdcan.h
index d9eed3efc..b0931e226 100644
--- a/panda/board/drivers/fdcan.h
+++ b/panda/board/drivers/fdcan.h
@@ -74,9 +74,7 @@ void process_can(uint8_t can_number) {
         to_push.RDHR = to_send.RDHR;
         can_send_errs += can_push(&can_rx_q, &to_push) ? 0U : 1U;
 
-        if (can_tx_check_min_slots_free(MAX_CAN_MSGS_PER_BULK_TRANSFER)) {
-          usb_outep3_resume_if_paused();
-        }
+        usb_cb_ep3_out_complete();
       }
     }
 
diff --git a/panda/board/drivers/uart.h b/panda/board/drivers/uart.h
index ecb3241a7..d6107a6c3 100644
--- a/panda/board/drivers/uart.h
+++ b/panda/board/drivers/uart.h
@@ -181,18 +181,23 @@ void putui(uint32_t i) {
   puts(&str[idx + 1U]);
 }
 
-void puth(unsigned int i) {
+void puthx(uint32_t i, uint8_t len) {
   const char c[] = "0123456789abcdef";
-  for (int pos = 28; pos != -4; pos -= 4) {
+  for (int pos = ((int)len * 4) - 4; pos > -4; pos -= 4) {
     putch(c[(i >> (unsigned int)(pos)) & 0xFU]);
   }
 }
 
+void puth(unsigned int i) {
+  puthx(i, 8U);
+}
+
 void puth2(unsigned int i) {
-  const char c[] = "0123456789abcdef";
-  for (int pos = 4; pos != -4; pos -= 4) {
-    putch(c[(i >> (unsigned int)(pos)) & 0xFU]);
-  }
+  puthx(i, 2U);
+}
+
+void puth4(unsigned int i) {
+  puthx(i, 4U);
 }
 
 void hexdump(const void *a, int l) {
diff --git a/panda/board/drivers/usb.h b/panda/board/drivers/usb.h
index abe88c7e1..2316f7220 100644
--- a/panda/board/drivers/usb.h
+++ b/panda/board/drivers/usb.h
@@ -823,9 +823,11 @@ void usb_irqhandler(void) {
       // USBx_OUTEP(3)->DOEPTSIZ = (1U << 19) | 0x40U;
       // USBx_OUTEP(3)->DOEPCTL |= USB_OTG_DOEPCTL_CNAK;
     } else if ((USBx_OUTEP(3)->DOEPINT) != 0) {
-      puts("OUTEP3 error ");
-      puth(USBx_OUTEP(3)->DOEPINT);
-      puts("\n");
+      #ifdef DEBUG_USB
+        puts("OUTEP3 error ");
+        puth(USBx_OUTEP(3)->DOEPINT);
+        puts("\n");
+      #endif
     } else {
       // USBx_OUTEP(3)->DOEPINT is 0, ok to skip
     }
diff --git a/panda/board/flash_h7.sh b/panda/board/flash_h7.sh
index 9b7566189..a7372c3f3 100755
--- a/panda/board/flash_h7.sh
+++ b/panda/board/flash_h7.sh
@@ -1,5 +1,5 @@
 #!/usr/bin/env sh
 set -e
 
-PANDA_GEN3=1 scons -u
+scons -u
 PYTHONPATH=.. python3 -c "from python import Panda; Panda().flash('obj/panda_h7.bin.signed')"
diff --git a/panda/board/main.c b/panda/board/main.c
index 5ace83530..9ac7bd90f 100644
--- a/panda/board/main.c
+++ b/panda/board/main.c
@@ -679,9 +679,10 @@ void tick_handler(void) {
       }
       #ifdef DEBUG
         puts("** blink ");
-        puth(can_rx_q.r_ptr); puts(" "); puth(can_rx_q.w_ptr); puts("  ");
-        puth(can_tx1_q.r_ptr); puts(" "); puth(can_tx1_q.w_ptr); puts("  ");
-        puth(can_tx2_q.r_ptr); puts(" "); puth(can_tx2_q.w_ptr); puts("\n");
+        puts("rx:"); puth4(can_rx_q.r_ptr); puts("-"); puth4(can_rx_q.w_ptr); puts("  ");
+        puts("tx1:"); puth4(can_tx1_q.r_ptr); puts("-"); puth4(can_tx1_q.w_ptr); puts("  ");
+        puts("tx2:"); puth4(can_tx2_q.r_ptr); puts("-"); puth4(can_tx2_q.w_ptr); puts("  ");
+        puts("tx3:"); puth4(can_tx3_q.r_ptr); puts("-"); puth4(can_tx3_q.w_ptr); puts("\n");
       #endif
 
       // Tick drivers
diff --git a/panda/board/main_declarations.h b/panda/board/main_declarations.h
index bbfb1a126..c211e5988 100644
--- a/panda/board/main_declarations.h
+++ b/panda/board/main_declarations.h
@@ -2,6 +2,7 @@
 void puts(const char *a);
 void puth(unsigned int i);
 void puth2(unsigned int i);
+void puth4(unsigned int i);
 typedef struct board board;
 typedef struct harness_configuration harness_configuration;
 void can_flip_buses(uint8_t bus1, uint8_t bus2);
diff --git a/panda/board/pedal/main_declarations.h b/panda/board/pedal/main_declarations.h
index 2f2fe6fee..13bf7ebc0 100644
--- a/panda/board/pedal/main_declarations.h
+++ b/panda/board/pedal/main_declarations.h
@@ -2,6 +2,7 @@
 void puts(const char *a);
 void puth(unsigned int i);
 void puth2(unsigned int i);
+void puth4(unsigned int i);
 typedef struct board board;
 typedef struct harness_configuration harness_configuration;
 
diff --git a/panda/board/recover_h7.sh b/panda/board/recover_h7.sh
index 737f954a9..1ca11884f 100755
--- a/panda/board/recover_h7.sh
+++ b/panda/board/recover_h7.sh
@@ -3,7 +3,7 @@ set -e
 
 DFU_UTIL="dfu-util"
 
-PANDA_H7=1 scons -u
+scons -u
 
 PYTHONPATH=.. python3 -c "from python import Panda; Panda().reset(enter_bootstub=True); Panda().reset(enter_bootloader=True)" || true
 sleep 1
diff --git a/panda/board/safety/safety_honda.h b/panda/board/safety/safety_honda.h
index e810015e9..82a48d635 100644
--- a/panda/board/safety/safety_honda.h
+++ b/panda/board/safety/safety_honda.h
@@ -323,8 +323,12 @@ static const addr_checks* honda_bosch_giraffe_init(int16_t param) {
   honda_hw = HONDA_BG_HW;
   // Checking for alternate brake override from safety parameter
   honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
+
   // radar disabled so allow gas/brakes
+#ifdef ALLOW_DEBUG
   honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
+#endif
+
   honda_rx_checks = (addr_checks){honda_addr_checks, HONDA_ADDR_CHECKS_LEN};
   return &honda_rx_checks;
 }
@@ -335,8 +339,12 @@ static const addr_checks* honda_bosch_harness_init(int16_t param) {
   honda_hw = HONDA_BH_HW;
   // Checking for alternate brake override from safety parameter
   honda_alt_brake_msg = GET_FLAG(param, HONDA_PARAM_ALT_BRAKE);
+
   // radar disabled so allow gas/brakes
+#ifdef ALLOW_DEBUG
   honda_bosch_long = GET_FLAG(param, HONDA_PARAM_BOSCH_LONG);
+#endif
+
   honda_rx_checks = (addr_checks){honda_bh_addr_checks, HONDA_BH_ADDR_CHECKS_LEN};
   return &honda_rx_checks;
 }
diff --git a/panda/board/safety/safety_hyundai.h b/panda/board/safety/safety_hyundai.h
index 286724c3b..6ed94c8ac 100644
--- a/panda/board/safety/safety_hyundai.h
+++ b/panda/board/safety/safety_hyundai.h
@@ -6,15 +6,28 @@ const int HYUNDAI_MAX_RATE_DOWN = 7;
 const int HYUNDAI_DRIVER_TORQUE_ALLOWANCE = 50;
 const int HYUNDAI_DRIVER_TORQUE_FACTOR = 2;
 const int HYUNDAI_STANDSTILL_THRSLD = 30;  // ~1kph
+
+const int HYUNDAI_MAX_ACCEL = 200;  // 1/100 m/s2
+const int HYUNDAI_MIN_ACCEL = -350; // 1/100 m/s2
+
 const CanMsg HYUNDAI_TX_MSGS[] = {
   {832, 0, 8},  // LKAS11 Bus 0
   {1265, 0, 4}, // CLU11 Bus 0
   {1157, 0, 4}, // LFAHDA_MFC Bus 0
-  // {1056, 0, 8}, //   SCC11,  Bus 0
-  // {1057, 0, 8}, //   SCC12,  Bus 0
-  // {1290, 0, 8}, //   SCC13,  Bus 0
-  // {905, 0, 8},  //   SCC14,  Bus 0
-  // {1186, 0, 8}  //   4a2SCC, Bus 0
+ };
+
+const CanMsg HYUNDAI_LONG_TX_MSGS[] = {
+  {832, 0, 8},  // LKAS11 Bus 0
+  {1265, 0, 4}, // CLU11 Bus 0
+  {1157, 0, 4}, // LFAHDA_MFC Bus 0
+  {1056, 0, 8}, // SCC11 Bus 0
+  {1057, 0, 8}, // SCC12 Bus 0
+  {1290, 0, 8}, // SCC13 Bus 0
+  {905, 0, 8},  // SCC14 Bus 0
+  {1186, 0, 2}, // FRT_RADAR11 Bus 0
+  {909, 0, 8},  // FCA11 Bus 0
+  {1155, 0, 8}, // FCA12 Bus 0
+  {2000, 0, 8}, // radar UDS TX addr Bus 0 (for radar disable)
  };
 
 AddrCheckStruct hyundai_addr_checks[] = {
@@ -26,6 +39,15 @@ AddrCheckStruct hyundai_addr_checks[] = {
 };
 #define HYUNDAI_ADDR_CHECK_LEN (sizeof(hyundai_addr_checks) / sizeof(hyundai_addr_checks[0]))
 
+AddrCheckStruct hyundai_long_addr_checks[] = {
+  {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
+           {881, 0, 8, .expected_timestep = 10000U}, { 0 }}},
+  {.msg = {{902, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{916, 0, 8, .check_checksum = true, .max_counter = 7U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{1265, 0, 4, .check_checksum = false, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+};
+#define HYUNDAI_LONG_ADDR_CHECK_LEN (sizeof(hyundai_long_addr_checks) / sizeof(hyundai_long_addr_checks[0]))
+
 // older hyundai models have less checks due to missing counters and checksums
 AddrCheckStruct hyundai_legacy_addr_checks[] = {
   {.msg = {{608, 0, 8, .check_checksum = true, .max_counter = 3U, .expected_timestep = 10000U},
@@ -38,10 +60,12 @@ AddrCheckStruct hyundai_legacy_addr_checks[] = {
 
 const int HYUNDAI_PARAM_EV_GAS = 1;
 const int HYUNDAI_PARAM_HYBRID_GAS = 2;
+const int HYUNDAI_PARAM_LONGITUDINAL = 4;
 
 bool hyundai_legacy = false;
 bool hyundai_ev_gas_signal = false;
 bool hyundai_hybrid_gas_signal = false;
+bool hyundai_longitudinal = false;
 
 addr_checks hyundai_rx_checks = {hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
 
@@ -57,6 +81,8 @@ static uint8_t hyundai_get_counter(CAN_FIFOMailBox_TypeDef *to_push) {
     cnt = (GET_BYTE(to_push, 1) >> 5) & 0x7;
   } else if (addr == 1057) {
     cnt = GET_BYTE(to_push, 7) & 0xF;
+  } else if (addr == 1265) {
+    cnt = (GET_BYTE(to_push, 3) >> 4) & 0xF;
   } else {
     cnt = 0;
   }
@@ -132,17 +158,35 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       update_sample(&torque_driver, torque_driver_new);
     }
 
-    // enter controls on rising edge of ACC, exit controls on ACC off
-    if (addr == 1057) {
-      // 2 bits: 13-14
-      int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
-      if (cruise_engaged && !cruise_engaged_prev) {
-        controls_allowed = 1;
+    if (hyundai_longitudinal) {
+      // ACC steering wheel buttons
+      if (addr == 1265) {
+        int button = GET_BYTE(to_push, 0) & 0x7;
+        switch (button) {
+          case 1:  // resume
+          case 2:  // set
+            controls_allowed = 1;
+            break;
+          case 4:  // cancel
+            controls_allowed = 0;
+            break;
+          default:
+            break;  // any other button is irrelevant
+        }
       }
-      if (!cruise_engaged) {
-        controls_allowed = 0;
+    } else {
+      // enter controls on rising edge of ACC, exit controls on ACC off
+      if (addr == 1057) {
+        // 2 bits: 13-14
+        int cruise_engaged = (GET_BYTES_04(to_push) >> 13) & 0x3;
+        if (cruise_engaged && !cruise_engaged_prev) {
+          controls_allowed = 1;
+        }
+        if (!cruise_engaged) {
+          controls_allowed = 0;
+        }
+        cruise_engaged_prev = cruise_engaged;
       }
-      cruise_engaged_prev = cruise_engaged;
     }
 
     // read gas pressed signal
@@ -167,7 +211,14 @@ static int hyundai_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       brake_pressed = (GET_BYTE(to_push, 6) >> 7) != 0;
     }
 
-    generic_rx_checks((addr == 832));
+    bool stock_ecu_detected = (addr == 832);
+
+    // If openpilot is controlling longitudinal we need to ensure the radar is turned off
+    // Enforce by checking we don't see SCC12
+    if (hyundai_longitudinal && (addr == 1057)) {
+      stock_ecu_detected = true;
+    }
+    generic_rx_checks(stock_ecu_detected);
   }
   return valid;
 }
@@ -177,14 +228,53 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
   int tx = 1;
   int addr = GET_ADDR(to_send);
 
-  if (!msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]))) {
-    tx = 0;
+  if (hyundai_longitudinal) {
+    tx = msg_allowed(to_send, HYUNDAI_LONG_TX_MSGS, sizeof(HYUNDAI_LONG_TX_MSGS)/sizeof(HYUNDAI_LONG_TX_MSGS[0]));
+  } else {
+    tx = msg_allowed(to_send, HYUNDAI_TX_MSGS, sizeof(HYUNDAI_TX_MSGS)/sizeof(HYUNDAI_TX_MSGS[0]));
   }
 
   if (relay_malfunction) {
     tx = 0;
   }
 
+  // FCA11: Block any potential actuation
+  if (addr == 909) {
+    int CR_VSM_DecCmd = GET_BYTE(to_send, 1);
+    int FCA_CmdAct = (GET_BYTE(to_send, 2) >> 5) & 1;
+    int CF_VSM_DecCmdAct = (GET_BYTE(to_send, 3) >> 7) & 1;
+
+    if ((CR_VSM_DecCmd != 0) || (FCA_CmdAct != 0) || (CF_VSM_DecCmdAct != 0)) {
+      tx = 0;
+    }
+  }
+
+  // ACCEL: safety check
+  if (addr == 1057) {
+    int desired_accel_raw = (((GET_BYTE(to_send, 4) & 0x7) << 8) | GET_BYTE(to_send, 3)) - 1023;
+    int desired_accel_val = ((GET_BYTE(to_send, 5) << 3) | (GET_BYTE(to_send, 4) >> 5)) - 1023;
+
+    int aeb_decel_cmd = GET_BYTE(to_send, 2);
+    int aeb_req = (GET_BYTE(to_send, 6) >> 6) & 1;
+
+    bool violation = 0;
+
+    if (!controls_allowed) {
+      if ((desired_accel_raw != 0) || (desired_accel_val != 0)) {
+        violation = 1;
+      }
+    }
+    violation |= max_limit_check(desired_accel_raw, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
+    violation |= max_limit_check(desired_accel_val, HYUNDAI_MAX_ACCEL, HYUNDAI_MIN_ACCEL);
+
+    violation |= (aeb_decel_cmd != 0);
+    violation |= (aeb_req != 0);
+
+    if (violation) {
+      tx = 0;
+    }
+  }
+
   // LKA STEER: safety check
   if (addr == 832) {
     int desired_torque = ((GET_BYTES_04(to_send) >> 16) & 0x7ff) - 1024;
@@ -232,6 +322,13 @@ static int hyundai_tx_hook(CAN_FIFOMailBox_TypeDef *to_send) {
     }
   }
 
+  // UDS: Only tester present ("\x02\x3E\x80\x00\x00\x00\x00\x00") allowed on diagnostics address
+  if (addr == 2000) {
+    if ((GET_BYTES_04(to_send) != 0x00803E02) || (GET_BYTES_48(to_send) != 0x0)) {
+      tx = 0;
+    }
+  }
+
   // FORCE CANCEL: safety check only relevant when spamming the cancel button.
   // ensuring that only the cancel button press is sent (VAL 4) when controls are off.
   // This avoids unintended engagements while still allowing resume spam
@@ -268,7 +365,16 @@ static const addr_checks* hyundai_init(int16_t param) {
   hyundai_legacy = false;
   hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
   hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
-  hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+
+#ifdef ALLOW_DEBUG
+  hyundai_longitudinal = GET_FLAG(param, HYUNDAI_PARAM_LONGITUDINAL);
+#endif
+
+  if (hyundai_longitudinal) {
+    hyundai_rx_checks = (addr_checks){hyundai_long_addr_checks, HYUNDAI_LONG_ADDR_CHECK_LEN};
+  } else {
+    hyundai_rx_checks = (addr_checks){hyundai_addr_checks, HYUNDAI_ADDR_CHECK_LEN};
+  }
   return &hyundai_rx_checks;
 }
 
@@ -277,6 +383,7 @@ static const addr_checks* hyundai_legacy_init(int16_t param) {
   relay_malfunction_reset();
 
   hyundai_legacy = true;
+  hyundai_longitudinal = false;
   hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS);
   hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS);
   hyundai_rx_checks = (addr_checks){hyundai_legacy_addr_checks, HYUNDAI_LEGACY_ADDR_CHECK_LEN};
diff --git a/panda/board/safety/safety_subaru.h b/panda/board/safety/safety_subaru.h
index faff35a3d..cde90980c 100644
--- a/panda/board/safety/safety_subaru.h
+++ b/panda/board/safety/safety_subaru.h
@@ -18,8 +18,8 @@ const CanMsg SUBARU_TX_MSGS[] = {{0x122, 0, 8}, {0x221, 0, 8}, {0x322, 0, 8}};
 AddrCheckStruct subaru_addr_checks[] = {
   {.msg = {{ 0x40, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
   {.msg = {{0x119, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
-  {.msg = {{0x139, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
   {.msg = {{0x13a, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
+  {.msg = {{0x13c, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 20000U}, { 0 }, { 0 }}},
   {.msg = {{0x240, 0, 8, .check_checksum = true, .max_counter = 15U, .expected_timestep = 50000U}, { 0 }, { 0 }}},
 };
 #define SUBARU_ADDR_CHECK_LEN (sizeof(subaru_addr_checks) / sizeof(subaru_addr_checks[0]))
@@ -89,8 +89,8 @@ static int subaru_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
       vehicle_moving = subaru_speed > SUBARU_STANDSTILL_THRSLD;
     }
 
-    if (addr == 0x139) {
-      brake_pressed = (GET_BYTES_48(to_push) & 0xFFF0) > 0;
+    if (addr == 0x13c) {
+      brake_pressed = ((GET_BYTE(to_push, 7) >> 6) & 1);
     }
 
     if (addr == 0x40) {
diff --git a/panda/board/safety/safety_volkswagen.h b/panda/board/safety/safety_volkswagen.h
index 67372757d..3332fe20a 100644
--- a/panda/board/safety/safety_volkswagen.h
+++ b/panda/board/safety/safety_volkswagen.h
@@ -37,7 +37,7 @@ addr_checks volkswagen_mqb_rx_checks = {volkswagen_mqb_addr_checks, VOLKSWAGEN_M
 #define MSG_MOTOR_2     0x288   // RX from ECU, for CC state and brake switch state
 #define MSG_MOTOR_3     0x380   // RX from ECU, for driver throttle input
 #define MSG_GRA_NEU     0x38A   // TX by OP, ACC control buttons for cancel/resume
-#define MSG_BREMSE_3    0x4A0   // RX from ABS, for wheel speeds
+#define MSG_BREMSE_1    0x1A0   // RX from ABS, for ego speed
 #define MSG_LDW_1       0x5BE   // TX by OP, Lane line recognition and text alerts
 
 // Transmit of GRA_Neu is allowed on bus 0 and 2 to keep compatibility with gateway and camera integration
@@ -48,7 +48,7 @@ AddrCheckStruct volkswagen_pq_addr_checks[] = {
   {.msg = {{MSG_LENKHILFE_3, 0, 6, .check_checksum = true,  .max_counter = 15U, .expected_timestep = 10000U}, { 0 }, { 0 }}},
   {.msg = {{MSG_MOTOR_2, 0, 8, .check_checksum = false, .max_counter = 0U,  .expected_timestep = 20000U}, { 0 }, { 0 }}},
   {.msg = {{MSG_MOTOR_3, 0, 8, .check_checksum = false, .max_counter = 0U,  .expected_timestep = 10000U}, { 0 }, { 0 }}},
-  {.msg = {{MSG_BREMSE_3, 0, 8, .check_checksum = false, .max_counter = 0U,  .expected_timestep = 10000U}, { 0 }, { 0 }}},
+  {.msg = {{MSG_BREMSE_1, 0, 8, .check_checksum = false, .max_counter = 0U,  .expected_timestep = 10000U}, { 0 }, { 0 }}},
 };
 #define VOLKSWAGEN_PQ_ADDR_CHECKS_LEN (sizeof(volkswagen_pq_addr_checks) / sizeof(volkswagen_pq_addr_checks[0]))
 addr_checks volkswagen_pq_rx_checks = {volkswagen_pq_addr_checks, VOLKSWAGEN_PQ_ADDR_CHECKS_LEN};
@@ -208,15 +208,12 @@ static int volkswagen_pq_rx_hook(CAN_FIFOMailBox_TypeDef *to_push) {
   if (valid && (GET_BUS(to_push) == 0)) {
     int addr = GET_ADDR(to_push);
 
-    // Update in-motion state by sampling front wheel speeds
-    // Signal: Bremse_3.Radgeschw__VL_4_1 (front left)
-    // Signal: Bremse_3.Radgeschw__VR_4_1 (front right)
-    if (addr == MSG_BREMSE_3) {
-      int wheel_speed_fl = (GET_BYTE(to_push, 0) | (GET_BYTE(to_push, 1) << 8)) >> 1;
-      int wheel_speed_fr = (GET_BYTE(to_push, 2) | (GET_BYTE(to_push, 3) << 8)) >> 1;
-      // Check for average front speed in excess of 0.3m/s, 1.08km/h
-      // DBC speed scale 0.01: 0.3m/s = 108, sum both wheels to compare
-      vehicle_moving = (wheel_speed_fl + wheel_speed_fr) > 216;
+    // Update in-motion state from speed value.
+    // Signal: Bremse_1.Geschwindigkeit_neu__Bremse_1_
+    if (addr == MSG_BREMSE_1) {
+      int speed = ((GET_BYTE(to_push, 2) & 0xFE) >> 1) | (GET_BYTE(to_push, 3) << 7);
+      // DBC speed scale 0.01: 0.3m/s = 108.
+      vehicle_moving = speed > 108;
     }
 
     // Update driver input torque samples
diff --git a/panda/examples/query_fw_versions.py b/panda/examples/query_fw_versions.py
index 6795f62c2..4132959be 100755
--- a/panda/examples/query_fw_versions.py
+++ b/panda/examples/query_fw_versions.py
@@ -6,7 +6,7 @@ from panda.python.uds import UdsClient, MessageTimeoutError, NegativeResponseErr
 
 if __name__ == "__main__":
   parser = argparse.ArgumentParser()
-  parser.add_argument('--rxoffset', default="0x8")
+  parser.add_argument('--rxoffset', default="")
   parser.add_argument('--nonstandard', action='store_true')
   parser.add_argument('--debug', action='store_true')
   parser.add_argument('--addr')
@@ -32,7 +32,6 @@ if __name__ == "__main__":
 
   panda = Panda()
   panda.set_safety_mode(Panda.SAFETY_ELM327)
-  panda.set_power_save(0)
   print("querying addresses ...")
   with tqdm(addrs) as t:
     for addr in t:
@@ -40,8 +39,11 @@ if __name__ == "__main__":
       if addr == 0x7df or addr == 0x18db33f1:
         continue
       t.set_description(hex(addr))
+      panda.send_heartbeat()
 
-      uds_client = UdsClient(panda, addr, addr + int(args.rxoffset, base=16), bus=1 if panda.has_obd() else 0, timeout=0.2, debug=args.debug)
+      bus = 1 if panda.has_obd() else 0
+      rx_addr = addr + int(args.rxoffset, base=16) if args.rxoffset else None
+      uds_client = UdsClient(panda, addr, rx_addr, bus, timeout=0.2, debug=args.debug)
       # Check for anything alive at this address, and switch to the highest
       # available diagnostic session without security access
       try:
diff --git a/panda/python/__init__.py b/panda/python/__init__.py
index 5d0d15186..be9ef492d 100644
--- a/panda/python/__init__.py
+++ b/panda/python/__init__.py
@@ -150,6 +150,7 @@ class Panda(object):
 
   FLAG_HONDA_ALT_BRAKE = 1
   FLAG_HONDA_BOSCH_LONG = 2
+  FLAG_HYUNDAI_LONG = 4
 
   def __init__(self, serial=None, claim=True):
     self._serial = serial
@@ -452,10 +453,11 @@ class Panda(object):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
     time.sleep(0.2)
 
-  def set_safety_mode(self, mode=SAFETY_SILENT, disable_heartbeat=True):
+  def set_safety_mode(self, mode=SAFETY_SILENT, disable_checks=True):
     self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, 0, b'')
-    if disable_heartbeat:
+    if disable_checks:
       self.set_heartbeat_disabled()
+      self.set_power_save(0)
 
   def set_can_forwarding(self, from_bus, to_bus):
     # TODO: This feature may not work correctly with saturated buses
diff --git a/panda/python/flash_release.py b/panda/python/flash_release.py
index 6e77d6a7f..ae50e6dc0 100755
--- a/panda/python/flash_release.py
+++ b/panda/python/flash_release.py
@@ -34,7 +34,7 @@ def flash_release(path=None, st_serial=None):
   zf = ZipFile(path)
   zf.printdir()
 
-  version = zf.read("version").decode()
+  version = zf.read("version").decode().strip()
   status("0. Preparing to flash " + str(version))
 
   code_bootstub = zf.read("bootstub.panda.bin")
diff --git a/panda/python/uds.py b/panda/python/uds.py
index 5106e6de8..48fcf5322 100644
--- a/panda/python/uds.py
+++ b/panda/python/uds.py
@@ -4,6 +4,7 @@ import struct
 from collections import deque
 from typing import Callable, NamedTuple, Tuple, List, Deque, Generator, Optional, cast
 from enum import IntEnum
+from functools import partial
 
 class SERVICE_TYPE(IntEnum):
   DIAGNOSTIC_SESSION_CONTROL = 0x10
@@ -376,39 +377,43 @@ class IsoTpMessage():
     self.rx_done = False
 
     if self.debug:
-      print(f"ISO-TP: REQUEST - 0x{bytes.hex(self.tx_dat)}")
+      print(f"ISO-TP: REQUEST - {hex(self._can_client.tx_addr)} 0x{bytes.hex(self.tx_dat)}")
     self._tx_first_frame()
 
   def _tx_first_frame(self) -> None:
     if self.tx_len < self.max_len:
       # single frame (send all bytes)
       if self.debug:
-        print("ISO-TP: TX - single frame")
+        print(f"ISO-TP: TX - single frame - {hex(self._can_client.tx_addr)}")
       msg = (bytes([self.tx_len]) + self.tx_dat).ljust(self.max_len, b"\x00")
       self.tx_done = True
     else:
       # first frame (send first 6 bytes)
       if self.debug:
-        print("ISO-TP: TX - first frame")
+        print(f"ISO-TP: TX - first frame - {hex(self._can_client.tx_addr)}")
       msg = (struct.pack("!H", 0x1000 | self.tx_len) + self.tx_dat[:self.max_len - 2]).ljust(self.max_len - 2, b"\x00")
     self._can_client.send([msg])
 
-  def recv(self) -> Optional[bytes]:
-    start_time = time.time()
+  def recv(self, timeout=None) -> Optional[bytes]:
+    if timeout is None:
+      timeout = self.timeout
+
+    start_time = time.monotonic()
     try:
       while True:
         for msg in self._can_client.recv():
           self._isotp_rx_next(msg)
+          start_time = time.monotonic()
           if self.tx_done and self.rx_done:
             return self.rx_dat
         # no timeout indicates non-blocking
-        if self.timeout == 0:
+        if timeout == 0:
           return None
-        if time.time() - start_time > self.timeout:
+        if time.monotonic() - start_time > timeout:
           raise MessageTimeoutError("timeout waiting for response")
     finally:
       if self.debug and self.rx_dat:
-        print(f"ISO-TP: RESPONSE - 0x{bytes.hex(self.rx_dat)}")
+        print(f"ISO-TP: RESPONSE - {hex(self._can_client.rx_addr)} 0x{bytes.hex(self.rx_dat)}")
 
   def _isotp_rx_next(self, rx_data: bytes) -> None:
     # single rx_frame
@@ -418,7 +423,7 @@ class IsoTpMessage():
       self.rx_idx = 0
       self.rx_done = True
       if self.debug:
-        print(f"ISO-TP: RX - single frame - idx={self.rx_idx} done={self.rx_done}")
+        print(f"ISO-TP: RX - single frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}")
       return
 
     # first rx_frame
@@ -428,9 +433,9 @@ class IsoTpMessage():
       self.rx_idx = 0
       self.rx_done = False
       if self.debug:
-        print(f"ISO-TP: RX - first frame - idx={self.rx_idx} done={self.rx_done}")
+        print(f"ISO-TP: RX - first frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}")
       if self.debug:
-        print("ISO-TP: TX - flow control continue")
+        print(f"ISO-TP: TX - flow control continue - {hex(self._can_client.tx_addr)}")
       # send flow control message (send all bytes)
       msg = b"\x30\x00\x00".ljust(self.max_len, b"\x00")
       self._can_client.send([msg])
@@ -446,7 +451,7 @@ class IsoTpMessage():
       if self.rx_len == len(self.rx_dat):
         self.rx_done = True
       if self.debug:
-        print(f"ISO-TP: RX - consecutive frame - idx={self.rx_idx} done={self.rx_done}")
+        print(f"ISO-TP: RX - consecutive frame - {hex(self._can_client.rx_addr)} idx={self.rx_idx} done={self.rx_done}")
       return
 
     # flow control
@@ -456,7 +461,7 @@ class IsoTpMessage():
       assert rx_data[0] == 0x30 or rx_data[0] == 0x31, "isotp - rx: flow-control transfer state indicator invalid"
       if rx_data[0] == 0x30:
         if self.debug:
-          print("ISO-TP: RX - flow control continue")
+          print(f"ISO-TP: RX - flow control continue - {hex(self._can_client.tx_addr)}")
         delay_ts = rx_data[2] & 0x7F
         # scale is 1 milliseconds if first bit == 0, 100 micro seconds if first bit == 1
         delay_div = 1000. if rx_data[2] & 0x80 == 0 else 10000.
@@ -478,11 +483,11 @@ class IsoTpMessage():
         if end >= self.tx_len:
           self.tx_done = True
         if self.debug:
-          print(f"ISO-TP: TX - consecutive frame - idx={self.tx_idx} done={self.tx_done}")
+          print(f"ISO-TP: TX - consecutive frame - {hex(self._can_client.tx_addr)} idx={self.tx_idx} done={self.tx_done}")
       elif rx_data[0] == 0x31:
         # wait (do nothing until next flow control message)
         if self.debug:
-          print("ISO-TP: TX - flow control wait")
+          print(f"ISO-TP: TX - flow control wait - {hex(self._can_client.tx_addr)}")
 
 FUNCTIONAL_ADDRS = [0x7DF, 0x18DB33F1]
 
@@ -503,13 +508,16 @@ def get_rx_addr_for_tx_addr(tx_addr, rx_offset=0x8):
 
 
 class UdsClient():
-  def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, timeout: float = 1, debug: bool = False):
+  def __init__(self, panda, tx_addr: int, rx_addr: int = None, bus: int = 0, timeout: float = 1, debug: bool = False,
+               tx_timeout: float = 1, response_pending_timeout: float = 10):
     self.bus = bus
     self.tx_addr = tx_addr
     self.rx_addr = rx_addr if rx_addr is not None else get_rx_addr_for_tx_addr(tx_addr)
     self.timeout = timeout
     self.debug = debug
-    self._can_client = CanClient(panda.can_send, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug)
+    can_send_with_timeout = partial(panda.can_send, timeout=int(tx_timeout*1000))
+    self._can_client = CanClient(can_send_with_timeout, panda.can_recv, self.tx_addr, self.rx_addr, self.bus, debug=self.debug)
+    self.response_pending_timeout = response_pending_timeout
 
   # generic uds request
   def _uds_request(self, service_type: SERVICE_TYPE, subfunction: int = None, data: bytes = None) -> bytes:
@@ -522,12 +530,15 @@ class UdsClient():
     # send request, wait for response
     isotp_msg = IsoTpMessage(self._can_client, self.timeout, self.debug)
     isotp_msg.send(req)
+    response_pending = False
     while True:
-      resp = isotp_msg.recv()
+      timeout = self.response_pending_timeout if response_pending else self.timeout
+      resp = isotp_msg.recv(timeout)
 
       if resp is None:
         continue
 
+      response_pending = False
       resp_sid = resp[0] if len(resp) > 0 else None
 
       # negative response
@@ -544,6 +555,7 @@ class UdsClient():
           error_desc = resp[3:].hex()
         # wait for another message if response pending
         if error_code == 0x78:
+          response_pending = True
           if self.debug:
             print("UDS-RX: response pending")
           continue
@@ -654,7 +666,7 @@ class UdsClient():
     resp = self._uds_request(SERVICE_TYPE.READ_DATA_BY_IDENTIFIER, subfunction=None, data=data)
     resp_id = struct.unpack('!H', resp[0:2])[0] if len(resp) >= 2 else None
     if resp_id != data_identifier_type:
-      raise ValueError('invalid response data identifier: {}'.format(hex(resp_id)))
+      raise ValueError('invalid response data identifier: {} expected: {}'.format(hex(resp_id), hex(data_identifier_type)))
     return resp[2:]
 
   def read_memory_by_address(self, memory_address: int, memory_size: int, memory_address_bytes: int = 4, memory_size_bytes: int = 1):
diff --git a/phonelibs/qpoases/EXAMPLES/example1.cpp b/phonelibs/qpoases/EXAMPLES/example1.cpp
deleted file mode 100644
index 92b47fc93..000000000
--- a/phonelibs/qpoases/EXAMPLES/example1.cpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file EXAMPLES/example1.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Very simple example for testing qpOASES (using QProblem class).
- */
-
-
-#include <QProblem.hpp>
-
-
-/** Example for qpOASES main function using the QProblem class. */
-int main( )
-{
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t A[1*2] = { 1.0, 1.0 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-	real_t lbA[1] = { -1.0 };
-	real_t ubA[1] = { 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-	real_t lbA_new[1] = { -2.0 };
-	real_t ubA_new[1] = { 1.0 };
-
-
-	/* Setting up QProblem object. */
-	QProblem example( 2,1 );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,A,lb,ub,lbA,ubA, nWSR,0 );
-
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new,lbA_new,ubA_new, nWSR,0 );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/EXAMPLES/example1b.cpp b/phonelibs/qpoases/EXAMPLES/example1b.cpp
deleted file mode 100644
index 331f19d8d..000000000
--- a/phonelibs/qpoases/EXAMPLES/example1b.cpp
+++ /dev/null
@@ -1,69 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file EXAMPLES/example1b.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3
- *	\date 2007-2008
- *
- *	Very simple example for testing qpOASES using the QProblemB class.
- */
-
-
-#include <QProblemB.hpp>
-
-
-/** Example for qpOASES main function using the QProblemB class. */
-int main( )
-{
-	/* Setup data of first QP. */
-	real_t H[2*2] = { 1.0, 0.0, 0.0, 0.5 };
-	real_t g[2] = { 1.5, 1.0 };
-	real_t lb[2] = { 0.5, -2.0 };
-	real_t ub[2] = { 5.0, 2.0 };
-
-	/* Setup data of second QP. */
-	real_t g_new[2] = { 1.0, 1.5 };
-	real_t lb_new[2] = { 0.0, -1.0 };
-	real_t ub_new[2] = { 5.0, -0.5 };
-
-
-	/* Setting up QProblemB object. */
-	QProblemB example( 2 );
-
-	/* Solve first QP. */
-	int nWSR = 10;
-	example.init( H,g,lb,ub, nWSR,0 );
-
-	/* Solve second QP. */
-	nWSR = 10;
-	example.hotstart( g_new,lb_new,ub_new, nWSR,0 );
-
-	return 0;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/Bounds.hpp b/phonelibs/qpoases/INCLUDE/Bounds.hpp
deleted file mode 100644
index 7260756da..000000000
--- a/phonelibs/qpoases/INCLUDE/Bounds.hpp
+++ /dev/null
@@ -1,189 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Bounds.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the Bounds class designed to manage working sets of
- *	bounds within a QProblem.
- */
-
-
-#ifndef QPOASES_BOUNDS_HPP
-#define QPOASES_BOUNDS_HPP
-
-
-#include <SubjectTo.hpp>
-
-
-
-/** This class manages working sets of bounds by storing
- *	index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class Bounds : public SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Bounds( );
-
-		/** Copy constructor (deep copy). */
-		Bounds(	const Bounds& rhs	/**< Rhs object. */
-				);
-
-		/** Destructor. */
-		~Bounds( );
-
-		/** Assignment operator (deep copy). */
-		Bounds& operator=(	const Bounds& rhs	/**< Rhs object. */
-							);
-
-
-		/** Pseudo-constructor takes the number of bounds.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue init(	int n	/**< Number of bounds. */
-							);
-
-
-		/** Initially adds number of a new (i.e. not yet in the list) bound to
-		 *  given index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupBound(	int _number,					/**< Number of new bound. */
-								SubjectToStatus _status			/**< Status of new bound. */
-								);
-
-		/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
-		 *  to the index set of free bounds; the order depends on the SujectToType
-		 *  of each index.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_BOUND_FAILED */
-		returnValue setupAllFree( );
-
-
-		/** Moves index of a bound from index list of fixed to that of free bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue moveFixedToFree(	int _number				/**< Number of bound to be freed. */
-										);
-
-		/** Moves index of a bound from index list of free to that of fixed bounds.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_BOUND_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue moveFreeToFixed(	int _number,			/**< Number of bound to be fixed. */
-										SubjectToStatus _status	/**< Status of bound to be fixed. */
-										);
-
-		/** Swaps the indices of two free bounds within the index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SWAPINDEX_FAILED */
-		returnValue swapFree(	int number1,					/**< Number of first constraint or bound. */
-								int number2						/**< Number of second constraint or bound. */
-								);
-
-
-		/** Returns number of variables.
-		 *	\return Number of variables. */
-		inline int getNV( ) const;
-
-		/** Returns number of implicitly fixed variables.
-		 *	\return Number of implicitly fixed variables. */
-		inline int getNFV( ) const;
-
-		/** Returns number of bounded (but possibly free) variables.
-		 *	\return Number of bounded (but possibly free) variables. */
-		inline int getNBV( ) const;
-
-		/** Returns number of unbounded variables.
-		 *	\return Number of unbounded variables. */
-		inline int getNUV( ) const;
-
-
-		/** Sets number of implicitly fixed variables.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNFV(	int n	/**< Number of implicitly fixed variables. */
-									);
-
-		/** Sets number of bounded (but possibly free) variables.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNBV(	int n	/**< Number of bounded (but possibly free) variables. */
-									);
-
-		/** Sets number of unbounded variables.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNUV(	int n	/**< Number of unbounded variables */
-									);
-
-
-		/** Returns number of free variables.
-		 *	\return Number of free variables. */
-		inline int getNFR( );
-
-		/** Returns number of fixed variables.
-		 *	\return Number of fixed variables. */
-		inline int getNFX( );
-
-
-		/** Returns a pointer to free variables index list.
-		 *	\return Pointer to free variables index list. */
-		inline Indexlist* getFree( );
-
-		/** Returns a pointer to fixed variables index list.
-		 *	\return Pointer to fixed variables index list. */
-		inline Indexlist* getFixed( );
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int nV;					/**< Number of variables (nV = nFV + nBV + nUV). */
-		int nFV;				/**< Number of implicitly fixed variables. */
-		int	nBV;				/**< Number of bounded (but possibly free) variables. */
-		int nUV;				/**< Number of unbounded variables. */
-
-		Indexlist free;			/**< Index list of free variables. */
-		Indexlist fixed;		/**< Index list of fixed variables. */
-};
-
-#include <Bounds.ipp>
-
-#endif	/* QPOASES_BOUNDS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/Constants.hpp b/phonelibs/qpoases/INCLUDE/Constants.hpp
deleted file mode 100644
index 92ee81229..000000000
--- a/phonelibs/qpoases/INCLUDE/Constants.hpp
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Constants.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2008
- *
- *	Definition of all global constants.
- */
-
-
-#ifndef QPOASES_CONSTANTS_HPP
-#define QPOASES_CONSTANTS_HPP
-
-#ifndef QPOASES_CUSTOM_INTERFACE
-#include "acado_qpoases_interface.hpp"
-#else
-  #define XSTR(x) #x
-  #define STR(x) XSTR(x)
-  #include STR(QPOASES_CUSTOM_INTERFACE)
-#endif
-
-/** Maximum number of variables within a QP formulation.
-	Note: this value has to be positive! */
-const int NVMAX = QPOASES_NVMAX;
-
-/** Maximum number of constraints within a QP formulation.
-	Note: this value has to be positive! */
-const int NCMAX = QPOASES_NCMAX;
-
-/** Redefinition of NCMAX used for memory allocation, to avoid zero sized arrays
-    and compiler errors. */
-const int NCMAX_ALLOC = (NCMAX == 0) ? 1 : NCMAX;
-
-/**< Maximum number of working set recalculations.
-	Note: this value has to be positive! */
-const int NWSRMAX = QPOASES_NWSRMAX;
-
-/** Desired KKT tolerance of QP solution; a warning RET_INACCURATE_SOLUTION is
- *  issued if this tolerance is not met.
- *	Note: this value has to be positive! */
-const real_t DESIREDACCURACY = (real_t) 1.0e-3;
-
-/** Critical KKT tolerance of QP solution; an error is issued if this
- *  tolerance is not met.
- *	Note: this value has to be positive! */
-const real_t CRITICALACCURACY = (real_t) 1.0e-2;
-
-
-
-/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
-	Note: this value has to be positive! */
-const real_t EPS = (real_t) QPOASES_EPS;
-
-/** Numerical value of zero (for situations in which it would be
- *	unreasonable to compare with 0.0).
- *	Note: this value has to be positive! */
-const real_t ZERO = (real_t) 1.0e-50;
-
-/** Numerical value of infinity (e.g. for non-existing bounds).
- *	Note: this value has to be positive! */
-const real_t INFTY = (real_t) 1.0e12;
-
-
-/** Lower/upper (constraints') bound tolerance (an inequality constraint
- *	whose lower and upper bound differ by less than BOUNDTOL is regarded
- *	to be an equality constraint).
- *	Note: this value has to be positive! */
-const real_t BOUNDTOL = (real_t) 1.0e-10;
-
-/** Offset for relaxing (constraints') bounds at beginning of an initial homotopy.
- *	Note: this value has to be positive! */
-const real_t BOUNDRELAXATION = (real_t) 1.0e3;
-
-
-/** Factor that determines physical lengths of index lists.
- *	Note: this value has to be greater than 1! */
-const int INDEXLISTFACTOR = 5;
-
-
-#endif	/* QPOASES_CONSTANTS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/Constraints.hpp b/phonelibs/qpoases/INCLUDE/Constraints.hpp
deleted file mode 100644
index 899167942..000000000
--- a/phonelibs/qpoases/INCLUDE/Constraints.hpp
+++ /dev/null
@@ -1,181 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Constraints.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the Constraints class designed to manage working sets of
- *	constraints within a QProblem.
- */
-
-
-#ifndef QPOASES_CONSTRAINTS_HPP
-#define QPOASES_CONSTRAINTS_HPP
-
-
-#include <SubjectTo.hpp>
-
-
-
-/** This class manages working sets of constraints by storing
- *	index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class Constraints : public SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Constraints( );
-
-		/** Copy constructor (deep copy). */
-		Constraints(	const Constraints& rhs	/**< Rhs object. */
-						);
-
-		/** Destructor. */
-		~Constraints( );
-
-		/** Assignment operator (deep copy). */
-		Constraints& operator=(	const Constraints& rhs	/**< Rhs object. */
-								);
-
-
-		/** Pseudo-constructor takes the number of constraints.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue init(	int n	/**< Number of constraints. */
-							);
-
-
-		/** Initially adds number of a new (i.e. not yet in the list) constraint to
-		 *  a given index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED \n
-					RET_INDEX_OUT_OF_BOUNDS \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupConstraint(	int _number,				/**< Number of new constraint. */
-										SubjectToStatus _status		/**< Status of new constraint. */
-										);
-
-		/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
-		 *  to the index set of inactive constraints; the order depends on the SujectToType
-		 *  of each index. Only disabled constraints are added to index set of disabled constraints!
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SETUP_CONSTRAINT_FAILED */
-		returnValue setupAllInactive( );
-
-
-		/** Moves index of a constraint from index list of active to that of inactive constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_CONSTRAINT_FAILED */
-		returnValue moveActiveToInactive(	int _number				/**< Number of constraint to become inactive. */
-											);
-
-		/** Moves index of a constraint from index list of inactive to that of active constraints.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MOVING_CONSTRAINT_FAILED */
-		returnValue moveInactiveToActive(	int _number,			/**< Number of constraint to become active. */
-											SubjectToStatus _status	/**< Status of constraint to become active. */
-											);
-
-
-		/** Returns the number of constraints.
-		 *	\return Number of constraints. */
-		inline int getNC( ) const;
-
-		/** Returns the number of implicit equality constraints.
-		 *	\return Number of implicit equality constraints. */
-		inline int getNEC( ) const;
-
-		/** Returns the number of "real" inequality constraints.
-		 *	\return Number of "real" inequality constraints. */
-		inline int getNIC( ) const;
-
-		/** Returns the number of unbounded constraints (i.e. without any bounds).
-		 *	\return Number of unbounded constraints (i.e. without any bounds). */
-		inline int getNUC( ) const;
-
-
-		/** Sets number of implicit equality constraints.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNEC(	int n	/**< Number of implicit equality constraints. */
-							);
-
-		/** Sets number of "real" inequality constraints.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNIC(	int n	/**< Number of "real" inequality constraints. */
-									);
-
-		/** Sets number of unbounded constraints (i.e. without any bounds).
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setNUC(	int n	/**< Number of unbounded constraints (i.e. without any bounds). */
-									);
-
-
-		/** Returns the number of active constraints.
-		 *	\return Number of constraints. */
-		inline int getNAC( );
-
-		/** Returns the number of inactive constraints.
-		 *	\return Number of constraints. */
-		inline int getNIAC( );
-
-
-		/** Returns a pointer to active constraints index list.
-		 *	\return Pointer to active constraints index list. */
-		inline Indexlist* getActive( );
-
-		/** Returns a pointer to inactive constraints index list.
-		 *	\return Pointer to inactive constraints index list. */
-		inline Indexlist* getInactive( );
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int nC;					/**< Number of constraints (nC = nEC + nIC + nUC). */
-		int nEC;				/**< Number of implicit equality constraints. */
-		int	nIC;				/**< Number of "real" inequality constraints. */
-		int nUC;				/**< Number of unbounded constraints (i.e. without any bounds). */
-
-		Indexlist active;		/**< Index list of active constraints. */
-		Indexlist inactive;		/**< Index list of inactive constraints. */
-};
-
-
-#include <Constraints.ipp>
-
-#endif	/* QPOASES_CONSTRAINTS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/CyclingManager.hpp b/phonelibs/qpoases/INCLUDE/CyclingManager.hpp
deleted file mode 100644
index b4410b106..000000000
--- a/phonelibs/qpoases/INCLUDE/CyclingManager.hpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/CyclingManager.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the CyclingManager class designed to detect
- *	and handle possible cycling during QP iterations.
- */
-
-
-#ifndef QPOASES_CYCLINGMANAGER_HPP
-#define QPOASES_CYCLINGMANAGER_HPP
-
-
-#include <Utils.hpp>
-
-
-
-/** This class is intended to detect and handle possible cycling during QP iterations.
- *	As cycling seems to occur quite rarely, this class is NOT FULLY IMPLEMENTED YET!
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class CyclingManager
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		CyclingManager( );
-
-		/** Copy constructor (deep copy). */
-		CyclingManager(	const CyclingManager& rhs	/**< Rhs object. */
-						);
-
-		/** Destructor. */
-		~CyclingManager( );
-
-		/** Copy asingment operator (deep copy). */
-		CyclingManager& operator=(	const CyclingManager& rhs	/**< Rhs object. */
-									);
-
-
-		/** Pseudo-constructor which takes the number of bounds/constraints.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue init(	int _nV,	/**< Number of bounds to be managed. */
-							int _nC		/**< Number of constraints to be managed. */
-							);
-
-
-		/** Stores index of a bound/constraint that might cause cycling.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		returnValue setCyclingStatus(	int number,				/**< Number of bound/constraint. */
-										BooleanType isBound,	/**< Flag that indicates if given number corresponds to a
-																 *   bound (BT_TRUE) or a constraint (BT_FALSE). */
-										CyclingStatus _status	/**< Cycling status of bound/constraint. */
-										);
-
-		/** Returns if bound/constraint might cause cycling.
-		 *	\return BT_TRUE: bound/constraint might cause cycling \n
-		 			BT_FALSE: otherwise */
-		CyclingStatus getCyclingStatus(	int number,			/**< Number of bound/constraint. */
-										BooleanType isBound	/**< Flag that indicates if given number corresponds to
-															 *   a bound (BT_TRUE) or a constraint (BT_FALSE). */
-										) const;
-
-
-		/** Clears all previous cycling information.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue clearCyclingData( );
-
-
-		/** Returns if cycling was detected.
-		 *	\return BT_TRUE iff cycling was detected. */
-		inline BooleanType isCyclingDetected( ) const;
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int	nV;									/**< Number of managed bounds. */
-		int	nC;									/**< Number of managed constraints. */
-
-		CyclingStatus status[NVMAX+NCMAX];		/**< Array to store cycling status of all bounds/constraints. */
-
-		BooleanType cyclingDetected;			/**< Flag if cycling was detected. */
-};
-
-
-#include <CyclingManager.ipp>
-
-#endif	/* QPOASES_CYCLINGMANAGER_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp b/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp
deleted file mode 100644
index 056bb26f0..000000000
--- a/phonelibs/qpoases/INCLUDE/EXTRAS/SolutionAnalysis.hpp
+++ /dev/null
@@ -1,107 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/EXTRAS/SolutionAnalysis.hpp
- *	\author Milan Vukov, Boris Houska, Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2012
- *
- *	Solution analysis class, based on a class in the standard version of the qpOASES
- */
-
-
-//
-
-#ifndef QPOASES_SOLUTIONANALYSIS_HPP
-#define QPOASES_SOLUTIONANALYSIS_HPP
-
-#include <QProblem.hpp>
-
-/** Enables the computation of variance as is in the standard version of qpOASES */
-#define QPOASES_USE_OLD_VERSION 0
-
-#if QPOASES_USE_OLD_VERSION
-#define KKT_DIM (2 * NVMAX + NCMAX)
-#endif
-
-class SolutionAnalysis
-{
-public:
-	
-	/** Default constructor. */
-	SolutionAnalysis( );
-	
-	/** Copy constructor (deep copy). */
-	SolutionAnalysis( 	const SolutionAnalysis& rhs	/**< Rhs object. */
-						);
-	
-	/** Destructor. */
-	~SolutionAnalysis( );
-	
-	/** Copy asingment operator (deep copy). */
-	SolutionAnalysis& operator=(	const SolutionAnalysis& rhs	/**< Rhs object. */
-									);
-	
-	/** A routine for computation of inverse of the Hessian matrix. */
-	returnValue getHessianInverse(
-									QProblem* qp,			/** QP */
-									real_t* hessianInverse	/** Inverse of the Hessian matrix*/
-									);
-	
-	/** A routine for computation of inverse of the Hessian matrix. */
-	returnValue getHessianInverse(	QProblemB* qp,			/** QP */
-									real_t* hessianInverse	/** Inverse of the Hessian matrix*/
-									);
-
-#if QPOASES_USE_OLD_VERSION
-	returnValue getVarianceCovariance(
-										QProblem* qp,
-										real_t* g_b_bA_VAR,
-										real_t* Primal_Dual_VAR
-										);
-#endif
-	
-private:
-	
-	real_t delta_g_cov[ NVMAX ];		/** A covariance-vector of g */
-	real_t delta_lb_cov[ NVMAX ];		/** A covariance-vector of lb */
-	real_t delta_ub_cov[ NVMAX ];		/** A covariance-vector of ub */
-	real_t delta_lbA_cov[ NCMAX_ALLOC ];		/** A covariance-vector of lbA */
-	real_t delta_ubA_cov[ NCMAX_ALLOC ];		/** A covariance-vector of ubA */
-	
-#if QPOASES_USE_OLD_VERSION
-	real_t K[KKT_DIM * KKT_DIM];		/** A matrix to store an intermediate result */
-#endif
-	
-	int FR_idx[ NVMAX ];				/** Index array for free variables */
-	int FX_idx[ NVMAX ];				/** Index array for fixed variables */
-	int AC_idx[ NCMAX_ALLOC ];				/** Index array for active constraints */
-	
-	real_t delta_xFR[ NVMAX ];			/** QP reaction, primal, w.r.t. free */
-	real_t delta_xFX[ NVMAX ];			/** QP reaction, primal, w.r.t. fixed */
-	real_t delta_yAC[ NVMAX ];			/** QP reaction, dual, w.r.t. active */
-	real_t delta_yFX[ NVMAX ];			/** QP reaction, dual, w.r.t. fixed*/
-};
-
-#endif // QPOASES_SOLUTIONANALYSIS_HPP
diff --git a/phonelibs/qpoases/INCLUDE/Indexlist.hpp b/phonelibs/qpoases/INCLUDE/Indexlist.hpp
deleted file mode 100644
index 21c31ddb9..000000000
--- a/phonelibs/qpoases/INCLUDE/Indexlist.hpp
+++ /dev/null
@@ -1,154 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Indexlist.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the Indexlist class designed to manage index lists of
- *	constraints and bounds within a SubjectTo object.
- */
-
-
-#ifndef QPOASES_INDEXLIST_HPP
-#define QPOASES_INDEXLIST_HPP
-
-
-#include <Utils.hpp>
-
-
-/** This class manages index lists.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class Indexlist
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		Indexlist( );
-
-		/** Copy constructor (deep copy). */
-		Indexlist(	const Indexlist& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~Indexlist( );
-
-		/** Assingment operator (deep copy). */
-		Indexlist& operator=(	const Indexlist& rhs	/**< Rhs object. */
-								);
-
-		/** Pseudo-constructor.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue init( );
-
-
-		/** Creates an array of all numbers within the index set in correct order.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_CORRUPTED */
-		returnValue	getNumberArray(	int* const numberarray	/**< Output: Array of numbers (NULL on error). */
-									) const;
-
-
-		/** Determines the index within the index list at with a given number is stored.
-		 *	\return >= 0: Index of given number. \n
-		 			-1: Number not found. */
-		int	getIndex(	int givennumber	/**< Number whose index shall be determined. */
-						) const;
-
-		/** Determines the physical index within the index list at with a given number is stored.
-		 *	\return >= 0: Index of given number. \n
-		 			-1: Number not found. */
-		int	getPhysicalIndex(	int givennumber	/**< Number whose physical index shall be determined. */
-								) const;
-
-		/** Returns the number stored at a given physical index.
-		 *	\return >= 0: Number stored at given physical index. \n
-		 			-RET_INDEXLIST_OUTOFBOUNDS */
-		int	getNumber(	int physicalindex	/**< Physical index of the number to be returned. */
-						) const;
-
-
-		/** Returns the current length of the index list.
-		 *	\return Current length of the index list. */
-		inline int getLength( );
-
-		/** Returns last number within the index list.
-		 *	\return Last number within the index list. */
-		inline int getLastNumber( ) const;
-
-
-		/** Adds number to index list.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_MUST_BE_REORDERD \n
-		 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
-		returnValue addNumber(	int addnumber	/**< Number to be added. */
-								);
-
-		/** Removes number from index list.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue removeNumber(	int removenumber	/**< Number to be removed. */
-									);
-
-		/** Swaps two numbers within index list.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue swapNumbers(	int number1,/**< First number for swapping. */
-									int number2	/**< Second number for swapping. */
-									);
-
-		/** Determines if a given number is contained in the index set.
-		 *	\return BT_TRUE iff number is contain in the index set */
-		inline BooleanType isMember(	int _number	/**< Number to be tested for membership. */
-										) const;
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		int number[INDEXLISTFACTOR*(NVMAX+NCMAX)];		/**< Array to store numbers of constraints or bounds. */
-		int next[INDEXLISTFACTOR*(NVMAX+NCMAX)];		/**< Array to store physical index of successor. */
-		int previous[INDEXLISTFACTOR*(NVMAX+NCMAX)];	/**< Array to store physical index of predecossor. */
-		int	length;										/**< Length of index list. */
-		int	first;										/**< Physical index of first element. */
-		int	last;										/**< Physical index of last element. */
-		int	lastusedindex;								/**< Physical index of last entry in index list. */
-		int	physicallength;								/**< Physical length of index list. */
-};
-
-
-#include <Indexlist.ipp>
-
-#endif	/* QPOASES_INDEXLIST_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/MessageHandling.hpp b/phonelibs/qpoases/INCLUDE/MessageHandling.hpp
deleted file mode 100644
index 3d17498ad..000000000
--- a/phonelibs/qpoases/INCLUDE/MessageHandling.hpp
+++ /dev/null
@@ -1,415 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/MessageHandling.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the MessageHandling class including global return values.
- */
-
-
-#ifndef QPOASES_MESSAGEHANDLING_HPP
-#define QPOASES_MESSAGEHANDLING_HPP
-
-// #define PC_DEBUG
-
-#ifdef PC_DEBUG
-  #include <stdio.h>
-
-  /** Defines an alias for  FILE  from stdio.h. */
-  #define myFILE FILE
-  /** Defines an alias for  stderr  from stdio.h. */
-  #define myStderr stderr
-  /** Defines an alias for  stdout  from stdio.h. */
-  #define myStdout stdout
-#else
-  /** Defines an alias for  FILE  from stdio.h. */
-  #define myFILE int
-  /** Defines an alias for  stderr  from stdio.h. */
-  #define myStderr 0
-  /** Defines an alias for  stdout  from stdio.h. */
-  #define myStdout 0
-#endif
-
-
-#include <Types.hpp>
-#include <Constants.hpp>
-
-
-/** Defines symbols for global return values. \n
- *  Important: All return values are assumed to be nonnegative! */
-enum returnValue
-{
-TERMINAL_LIST_ELEMENT = -1,						/**< Terminal list element, internal usage only! */
-/* miscellaneous */
-SUCCESSFUL_RETURN = 0,							/**< Successful return. */
-RET_DIV_BY_ZERO,		   						/**< Division by zero. */
-RET_INDEX_OUT_OF_BOUNDS,						/**< Index out of bounds. */
-RET_INVALID_ARGUMENTS,							/**< At least one of the arguments is invalid. */
-RET_ERROR_UNDEFINED,							/**< Error number undefined. */
-RET_WARNING_UNDEFINED,							/**< Warning number undefined. */
-RET_INFO_UNDEFINED,								/**< Info number undefined. */
-RET_EWI_UNDEFINED,								/**< Error/warning/info number undefined. */
-RET_AVAILABLE_WITH_LINUX_ONLY,					/**< This function is available under Linux only. */
-RET_UNKNOWN_BUG,								/**< The error occured is not yet known. */
-RET_PRINTLEVEL_CHANGED,							/**< 10 Print level changed. */
-RET_NOT_YET_IMPLEMENTED,						/**< Requested function is not yet implemented in this version of qpOASES. */
-/* Indexlist */
-RET_INDEXLIST_MUST_BE_REORDERD,					/**< Index list has to be reordered. */
-RET_INDEXLIST_EXCEEDS_MAX_LENGTH,				/**< Index list exceeds its maximal physical length. */
-RET_INDEXLIST_CORRUPTED,						/**< Index list corrupted. */
-RET_INDEXLIST_OUTOFBOUNDS,						/**< Physical index is out of bounds. */
-RET_INDEXLIST_ADD_FAILED,						/**< Adding indices from another index set failed. */
-RET_INDEXLIST_INTERSECT_FAILED,					/**< Intersection with another index set failed. */
-/* SubjectTo / Bounds / Constraints */
-RET_INDEX_ALREADY_OF_DESIRED_STATUS,			/**< Index is already of desired status. */
-RET_ADDINDEX_FAILED,							/**< Cannot swap between different indexsets. */
-RET_SWAPINDEX_FAILED,							/**< 20 Adding index to index set failed. */
-RET_NOTHING_TO_DO,								/**< Nothing to do. */
-RET_SETUP_BOUND_FAILED,							/**< Setting up bound index failed. */
-RET_SETUP_CONSTRAINT_FAILED,					/**< Setting up constraint index failed. */
-RET_MOVING_BOUND_FAILED,						/**< Moving bound between index sets failed. */
-RET_MOVING_CONSTRAINT_FAILED,					/**< Moving constraint between index sets failed. */
-/* QProblem */
-RET_QP_ALREADY_INITIALISED,						/**< QProblem has already been initialised. */
-RET_NO_INIT_WITH_STANDARD_SOLVER,				/**< Initialisation via extern QP solver is not yet implemented. */
-RET_RESET_FAILED,								/**< Reset failed. */
-RET_INIT_FAILED,								/**< Initialisation failed. */
-RET_INIT_FAILED_TQ,								/**< 30 Initialisation failed due to TQ factorisation. */
-RET_INIT_FAILED_CHOLESKY,						/**< Initialisation failed due to Cholesky decomposition. */
-RET_INIT_FAILED_HOTSTART,						/**< Initialisation failed! QP could not be solved! */
-RET_INIT_FAILED_INFEASIBILITY,					/**< Initial QP could not be solved due to infeasibility! */
-RET_INIT_FAILED_UNBOUNDEDNESS,					/**< Initial QP could not be solved due to unboundedness! */
-RET_INIT_SUCCESSFUL,							/**< Initialisation done. */
-RET_OBTAINING_WORKINGSET_FAILED,				/**< Failed to obtain working set for auxiliary QP. */
-RET_SETUP_WORKINGSET_FAILED,					/**< Failed to setup working set for auxiliary QP. */
-RET_SETUP_AUXILIARYQP_FAILED,					/**< Failed to setup auxiliary QP for initialised homotopy. */
-RET_NO_EXTERN_SOLVER,							/**< No extern QP solver available. */
-RET_QP_UNBOUNDED,								/**< 40 QP is unbounded. */
-RET_QP_INFEASIBLE,								/**< QP is infeasible. */
-RET_QP_NOT_SOLVED,								/**< Problems occured while solving QP with standard solver. */
-RET_QP_SOLVED,									/**< QP successfully solved. */
-RET_UNABLE_TO_SOLVE_QP,							/**< Problems occured while solving QP. */
-RET_INITIALISATION_STARTED,						/**< Starting problem initialisation. */
-RET_HOTSTART_FAILED,							/**< Unable to perform homotopy due to internal error. */
-RET_HOTSTART_FAILED_TO_INIT,					/**< Unable to initialise problem. */
-RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED,		/**< Unable to perform homotopy as previous QP is not solved. */
-RET_ITERATION_STARTED,							/**< Iteration... */
-RET_SHIFT_DETERMINATION_FAILED,					/**< 50 Determination of shift of the QP data failed. */
-RET_STEPDIRECTION_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
-RET_STEPLENGTH_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
-RET_OPTIMAL_SOLUTION_FOUND,						/**< Optimal solution of neighbouring QP found. */
-RET_HOMOTOPY_STEP_FAILED,						/**< Unable to perform homotopy step. */
-RET_HOTSTART_STOPPED_INFEASIBILITY,				/**< Premature homotopy termination because QP is infeasible. */
-RET_HOTSTART_STOPPED_UNBOUNDEDNESS,				/**< Premature homotopy termination because QP is unbounded. */
-RET_WORKINGSET_UPDATE_FAILED,					/**< Unable to update working sets according to initial guesses. */
-RET_MAX_NWSR_REACHED,							/**< Maximum number of working set recalculations performed. */
-RET_CONSTRAINTS_NOT_SPECIFIED,					/**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
-RET_INVALID_FACTORISATION_FLAG,					/**< 60 Invalid factorisation flag. */
-RET_UNABLE_TO_SAVE_QPDATA,						/**< Unable to save QP data. */
-RET_STEPDIRECTION_FAILED_TQ,					/**< Abnormal termination due to TQ factorisation. */
-RET_STEPDIRECTION_FAILED_CHOLESKY,				/**< Abnormal termination due to Cholesky factorisation. */
-RET_CYCLING_DETECTED,							/**< Cycling detected. */
-RET_CYCLING_NOT_RESOLVED,						/**< Cycling cannot be resolved, QP probably infeasible. */
-RET_CYCLING_RESOLVED,							/**< Cycling probably resolved. */
-RET_STEPSIZE,									/**< For displaying performed stepsize. */
-RET_STEPSIZE_NONPOSITIVE,						/**< For displaying non-positive stepsize. */
-RET_SETUPSUBJECTTOTYPE_FAILED,					/**< Setup of SubjectToTypes failed. */
-RET_ADDCONSTRAINT_FAILED,						/**< 70 Addition of constraint to working set failed. */
-RET_ADDCONSTRAINT_FAILED_INFEASIBILITY,			/**< Addition of constraint to working set failed (due to QP infeasibility). */
-RET_ADDBOUND_FAILED,							/**< Addition of bound to working set failed. */
-RET_ADDBOUND_FAILED_INFEASIBILITY,				/**< Addition of bound to working set failed (due to QP infeasibility). */
-RET_REMOVECONSTRAINT_FAILED,					/**< Removal of constraint from working set failed. */
-RET_REMOVEBOUND_FAILED,							/**< Removal of bound from working set failed. */
-RET_REMOVE_FROM_ACTIVESET,						/**< Removing from active set... */
-RET_ADD_TO_ACTIVESET,							/**< Adding to active set... */
-RET_REMOVE_FROM_ACTIVESET_FAILED,				/**< Removing from active set failed. */
-RET_ADD_TO_ACTIVESET_FAILED,					/**< Adding to active set failed. */
-RET_CONSTRAINT_ALREADY_ACTIVE,					/**< 80 Constraint is already active. */
-RET_ALL_CONSTRAINTS_ACTIVE,						/**< All constraints are active, no further constraint can be added. */
-RET_LINEARLY_DEPENDENT,							/**< New bound/constraint is linearly dependent. */
-RET_LINEARLY_INDEPENDENT,						/**< New bound/constraint is linearly independent. */
-RET_LI_RESOLVED,								/**< Linear independence of active contraint matrix successfully resolved. */
-RET_ENSURELI_FAILED,							/**< Failed to ensure linear indepence of active contraint matrix. */
-RET_ENSURELI_FAILED_TQ,							/**< Abnormal termination due to TQ factorisation. */
-RET_ENSURELI_FAILED_NOINDEX,					/**< No index found, QP probably infeasible. */
-RET_ENSURELI_FAILED_CYCLING,					/**< Cycling detected, QP probably infeasible. */
-RET_BOUND_ALREADY_ACTIVE,						/**< Bound is already active. */
-RET_ALL_BOUNDS_ACTIVE,							/**< 90 All bounds are active, no further bound can be added. */
-RET_CONSTRAINT_NOT_ACTIVE,						/**< Constraint is not active. */
-RET_BOUND_NOT_ACTIVE,							/**< Bound is not active. */
-RET_HESSIAN_NOT_SPD,							/**< Projected Hessian matrix not positive definite. */
-RET_MATRIX_SHIFT_FAILED,						/**< Unable to update matrices or to transform vectors. */
-RET_MATRIX_FACTORISATION_FAILED,				/**< Unable to calculate new matrix factorisations. */
-RET_PRINT_ITERATION_FAILED,						/**< Unable to print information on current iteration. */
-RET_NO_GLOBAL_MESSAGE_OUTPUTFILE,				/**< No global message output file initialised. */
-/* Utils */
-RET_UNABLE_TO_OPEN_FILE,						/**< Unable to open file. */
-RET_UNABLE_TO_WRITE_FILE,						/**< Unable to write into file. */
-RET_UNABLE_TO_READ_FILE,						/**< 100 Unable to read from file. */
-RET_FILEDATA_INCONSISTENT,						/**< File contains inconsistent data. */
-/* SolutionAnalysis */
-RET_NO_SOLUTION, 								/**< QP solution does not satisfy KKT optimality conditions. */
-RET_INACCURATE_SOLUTION							/**< KKT optimality conditions not satisfied to sufficient accuracy. */
-};
-
-
-
-/** This class handles all kinds of messages (errors, warnings, infos) initiated
- *  by qpOASES modules and stores the correspoding global preferences.
- *
- *	\author Hans Joachim Ferreau (special thanks to Leonard Wirsching)
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class MessageHandling
-{
-	/*
-	 *	INTERNAL DATA STRUCTURES
-	 */
-	public:
-		/** Data structure for entries in global message list. */
-		typedef struct {
-			returnValue key;							/**< Global return value. */
-			const char* data;							/**< Corresponding message. */
-			VisibilityStatus globalVisibilityStatus; 	/**< Determines if message can be printed.
-														* 	 If this value is set to VS_HIDDEN, no message is printed! */
-		} ReturnValueList;
-
-
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		MessageHandling( );
-
-		/** Constructor which takes the desired output file. */
-		MessageHandling(  myFILE* _outputFile					/**< Output file. */
-						  );
-
-		/** Constructor which takes the desired visibility states. */
-		MessageHandling(	VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
-							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
-							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
-							);
-
-		/** Constructor which takes the desired output file and desired visibility states. */
-		MessageHandling(	myFILE* _outputFile,				/**< Output file. */
-							VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
-							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
-							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
-							);
-
-		/** Copy constructor (deep copy). */
-		MessageHandling(	const MessageHandling& rhs	/**< Rhs object. */
-							);
-
-		/** Destructor. */
-		~MessageHandling( );
-
-		/** Assignment operator (deep copy). */
-		MessageHandling& operator=(	const MessageHandling& rhs	/**< Rhs object. */
-									);
-
-
-		/** Prints an error message(a simplified macro THROWERROR is also provided). \n
-		 *  Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
-		 *  Errors of a sub function should be commented by the calling function by means of a warning message
-		 *  (if this error does not cause an error of the calling function, either)!
-		 *  \return Error number returned by sub function call
-		 */
-		returnValue throwError(
-			returnValue Enumber,					/**< Error number returned by sub function call. */
-			const char* additionaltext,				/**< Additional error text (0, if none). */
-			const char* functionname,				/**< Name of function which caused the error. */
-			const char* filename,					/**< Name of file which caused the error. */
-			const unsigned long linenumber,			/**< Number of line which caused the error.incompatible binary file */
-			VisibilityStatus localVisibilityStatus	/**< Determines (locally) if error message can be printed to myStderr.
-			   									 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-	   			 								 *   no message is printed, anyway! */
-		);
-
-		/** Prints a warning message (a simplified macro THROWWARNING is also provided).
-		 *  Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
-		 *  \return Warning number returned by sub function call
-		 */
-		returnValue throwWarning(
-			returnValue Wnumber,	 				/**< Warning number returned by sub function call. */
-			const char* additionaltext,				/**< Additional warning text (0, if none). */
-			const char* functionname,				/**< Name of function which caused the warning. */
-			const char* filename,   				/**< Name of file which caused the warning. */
-			const unsigned long linenumber,	 		/**< Number of line which caused the warning. */
-			VisibilityStatus localVisibilityStatus	/**< Determines (locally) if warning message can be printed to myStderr.
-		   		  									 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-					 								 *   no message is printed, anyway! */
-			);
-
-	   /** Prints a info message (a simplified macro THROWINFO is also provided).
-		 *  \return Info number returned by sub function call
-		 */
-		returnValue throwInfo(
-			returnValue Inumber,	 				/**< Info number returned by sub function call. */
-			const char* additionaltext,	 			/**< Additional warning text (0, if none). */
-			const char* functionname,				/**< Name of function which submitted the info. */
-			const char* filename,   				/**< Name of file which submitted the info. */
-			const unsigned long linenumber,			/**< Number of line which submitted the info. */
-			VisibilityStatus localVisibilityStatus	/**< Determines (locally) if info message can be printed to myStderr.
-				  									 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-				   									 *   no message is printed, anyway! */
-			);
-
-
-		/** Resets all preferences to default values.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue reset( );
-
-
-		/** Prints a complete list of all messages to output file.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue listAllMessages( );
-
-
-		/** Returns visibility status for error messages.
-		 *	\return Visibility status for error messages. */
-		inline VisibilityStatus getErrorVisibilityStatus( ) const;
-
-		/** Returns visibility status for warning messages.
-		 *	\return Visibility status for warning messages. */
-		inline VisibilityStatus getWarningVisibilityStatus( ) const;
-
-		/** Returns visibility status for info messages.
-		 *	\return Visibility status for info messages. */
-		inline VisibilityStatus getInfoVisibilityStatus( ) const;
-
-		/** Returns pointer to output file.
-		 *	\return Pointer to output file. */
-		inline myFILE* getOutputFile( ) const;
-
-		/** Returns error count value.
-		 *	\return Error count value. */
-		inline int getErrorCount( ) const;
-
-
-		/** Changes visibility status for error messages. */
-		inline void setErrorVisibilityStatus(	VisibilityStatus _errorVisibility	/**< New visibility status for error messages. */
-												);
-
-		/** Changes visibility status for warning messages. */
-		inline void setWarningVisibilityStatus(	VisibilityStatus _warningVisibility	/**< New visibility status for warning messages. */
-												);
-
-		/** Changes visibility status for info messages. */
-		inline void setInfoVisibilityStatus(	VisibilityStatus _infoVisibility	/**< New visibility status for info messages. */
-												);
-
-		/** Changes output file for messages. */
-		inline void setOutputFile(	myFILE* _outputFile	/**< New output file for messages. */
-									);
-
-		/** Changes error count.
-		 * \return SUCCESSFUL_RETURN \n
-		 *		   RET_INVALID_ARGUMENT */
-		inline returnValue setErrorCount(	int _errorCount	/**< New error count value. */
-											);
-
-		/** Return the error code string. */
-		static const char* getErrorString(int error);
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Prints a info message to myStderr (auxiliary function).
-		 *  \return Error/warning/info number returned by sub function call
-		 */
-		returnValue throwMessage(
-			returnValue RETnumber,	 				/**< Error/warning/info number returned by sub function call. */
-			const char* additionaltext,				/**< Additional warning text (0, if none). */
-			const char* functionname,				/**< Name of function which caused the error/warning/info. */
-			const char* filename,   				/**< Name of file which caused the error/warning/info. */
-			const unsigned long linenumber,			/**< Number of line which caused the error/warning/info. */
-			VisibilityStatus localVisibilityStatus,	/**< Determines (locally) if info message can be printed to myStderr.
-					  								 *   If GLOBAL visibility status of the message is set to VS_HIDDEN,
-				   									 *   no message is printed, anyway! */
-			const char* RETstring					/**< Leading string of error/warning/info message. */
-			);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		VisibilityStatus errorVisibility;		/**< Error messages visible? */
-		VisibilityStatus warningVisibility;		/**< Warning messages visible? */
-		VisibilityStatus infoVisibility;		/**< Info messages visible? */
-
-		myFILE* outputFile;						/**< Output file for messages. */
-
-		int errorCount; 						/**< Counts number of errors (for nicer output only). */
-};
-
-
-#ifndef __FUNCTION__
-  /** Ensures that __FUNCTION__ macro is defined. */
-  #define __FUNCTION__ 0
-#endif
-
-#ifndef __FILE__
-  /** Ensures that __FILE__ macro is defined. */
-  #define __FILE__ 0
-#endif
-
-#ifndef __LINE__
-  /** Ensures that __LINE__ macro is defined. */
-  #define __LINE__ 0
-#endif
-
-
-/** Short version of throwError with default values, only returnValue is needed */
-#define THROWERROR(retval) ( getGlobalMessageHandler( )->throwError((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-/** Short version of throwWarning with default values, only returnValue is needed */
-#define THROWWARNING(retval) ( getGlobalMessageHandler( )->throwWarning((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-/** Short version of throwInfo with default values, only returnValue is needed */
-#define THROWINFO(retval) ( getGlobalMessageHandler( )->throwInfo((retval),0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE) )
-
-
-/** Returns a pointer to global message handler.
- *  \return Pointer to global message handler.
- */
-MessageHandling* getGlobalMessageHandler( );
-
-
-#include <MessageHandling.ipp>
-
-#endif /* QPOASES_MESSAGEHANDLING_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/QProblem.hpp b/phonelibs/qpoases/INCLUDE/QProblem.hpp
deleted file mode 100644
index 91dc43417..000000000
--- a/phonelibs/qpoases/INCLUDE/QProblem.hpp
+++ /dev/null
@@ -1,666 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/QProblem.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the QProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-
-#ifndef QPOASES_QPROBLEM_HPP
-#define QPOASES_QPROBLEM_HPP
-
-
-#include <QProblemB.hpp>
-#include <Constraints.hpp>
-#include <CyclingManager.hpp>
-
-
-/** A class for setting up and solving quadratic programs. The main feature is
- *	the possibily to use the newly developed online active set strategy for
- * 	parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class QProblem : public QProblemB
-{
-	/* allow SolutionAnalysis class to access private members */
-	friend class SolutionAnalysis;
-	
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		QProblem( );
-
-		/** Constructor which takes the QP dimensions only. */
-		QProblem(	int _nV,	  				/**< Number of variables. */
-					int _nC  					/**< Number of constraints. */
-					);
-
-		/** Copy constructor (deep copy). */
-		QProblem(	const QProblem& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~QProblem( );
-
-		/** Assignment operator (deep copy). */
-		QProblem& operator=(	const QProblem& rhs		/**< Rhs object. */
-								);
-
-
-		/** Clears all data structures of QProblemB except for QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_RESET_FAILED */
-		returnValue reset( );
-
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue init(	const real_t* const _H, 		/**< Hessian matrix. */
-							const real_t* const _g, 		/**< Gradient vector. */
-							const real_t* const _A,  		/**< Constraint matrix. */
-							const real_t* const _lb,		/**< Lower bound vector (on variables). \n
-																If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,		/**< Upper bound vector (on variables). \n
-																If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,		/**< Lower constraints' bound vector. \n
-																If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,		/**< Upper constraints' bound vector. \n
-																If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,						/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																Output: Number of performed working set recalculations. */
-							const real_t* const yOpt = 0,	/**< Initial guess for dual solution vector. */
-							real_t* const cputime = 0		/**< Output: CPU time required to initialise QP. */
-							);
-
-
-		/** Initialises a QProblem with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue init(	const real_t* const _H, 		/**< Hessian matrix. */
-							const real_t* const _R, 		/**< Cholesky factorization of the Hessian matrix. */
-							const real_t* const _g, 		/**< Gradient vector. */
-							const real_t* const _A,  		/**< Constraint matrix. */
-							const real_t* const _lb,		/**< Lower bound vector (on variables). \n
-																If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,		/**< Upper bound vector (on variables). \n
-																If no upper bounds exist, a NULL pointer can be passed. */
-							const real_t* const _lbA,		/**< Lower constraints' bound vector. \n
-																If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ubA,		/**< Upper constraints' bound vector. \n
-																If no lower constraints' bounds exist, a NULL pointer can be passed. */
-							int& nWSR,						/**< Input: Maximum number of working set recalculations when using initial homotopy.
-																Output: Number of performed working set recalculations. */
-							const real_t* const yOpt = 0,	/**< Initial guess for dual solution vector. */
-							real_t* const cputime = 0		/**< Output: CPU time required to initialise QP. */
-							);
-
-
-		/** Solves QProblem using online active set strategy.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_MAX_NWSR_REACHED \n
-		 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue hotstart(	const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper bounds exist, a NULL pointer can be passed. */
-								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
-													 			 	 If no upper constraints' bounds exist, a NULL pointer can be passed. */
-								int& nWSR,						/**< Input: Maximum number of working set recalculations; \n
-															 		 Output: Number of performed working set recalculations. */
-								real_t* const cputime			/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
-								);
-
-
-		/** Returns constraint matrix of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getA(	real_t* const _A	/**< Array of appropriate dimension for copying constraint matrix.*/
-									) const;
-
-		/** Returns a single row of constraint matrix of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue getA(	int number,			/**< Number of entry to be returned. */
-									real_t* const row	/**< Array of appropriate dimension for copying (number)th constraint. */
-									) const;
-
-		/** Returns lower constraints' bound vector of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getLBA(	real_t* const _lbA	/**< Array of appropriate dimension for copying lower constraints' bound vector.*/
-									) const;
-
-		/** Returns single entry of lower constraints' bound vector of the QP.
-		  *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue getLBA(	int number,		/**< Number of entry to be returned. */
-									real_t& value	/**< Output: lbA[number].*/
-									) const;
-
-		/** Returns upper constraints' bound vector of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getUBA(	real_t* const _ubA	/**< Array of appropriate dimension for copying upper constraints' bound vector.*/
-									) const;
-
-		/** Returns single entry of upper constraints' bound vector of the QP.
-		  *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue getUBA(	int number,		/**< Number of entry to be returned. */
-									real_t& value	/**< Output: ubA[number].*/
-									) const;
-
-
-		/** Returns current constraints object of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getConstraints(	Constraints* const _constraints	/** Output: Constraints object. */
-											) const;
-
-
-		/** Returns the number of constraints.
-		 *	\return Number of constraints. */
-		inline int getNC( ) const;
-
-		/** Returns the number of (implicitly defined) equality constraints.
-		 *	\return Number of (implicitly defined) equality constraints. */
-		inline int getNEC( ) const;
-
-		/** Returns the number of active constraints.
-		 *	\return Number of active constraints. */
-		inline int getNAC( );
-
-		/** Returns the number of inactive constraints.
-		 *	\return Number of inactive constraints. */
-		inline int getNIAC( );
-
-		/** Returns the dimension of null space.
-		 *	\return Dimension of null space. */
-		int getNZ( );
-
-
-		/** Returns the dual solution vector (deep copy).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
-										) const;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		returnValue setupSubjectToType( );
-
-		/** Computes the Cholesky decomposition R of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		returnValue setupCholeskyDecompositionProjected( );
-
-		/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEXLIST_CORRUPTED */
-		returnValue setupTQfactorisation( );
-
-
-		/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
-		 *  A guess for its primal/dual optimal solution vectors and the corresponding
-		 *  working sets of bounds and constraints can be provided.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_TQ \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED */
-		returnValue solveInitialQP(	const real_t* const xOpt,						/**< Optimal primal solution vector.
-																					 *	 A NULL pointer can be passed. */
-									const real_t* const yOpt,						/**< Optimal dual solution vector.
-																					 *	 A NULL pointer can be passed. */
-									const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt).
-																		 			 *	 A NULL pointer can be passed. */
-									const Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt).
-																		 			 *	 A NULL pointer can be passed. */
-									int& nWSR, 										/**< Input: Maximum number of working set recalculations; \n
-																 					 *	 Output: Number of performed working set recalculations. */
-									real_t* const cputime							/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
-									);
-
-		/** Obtains the desired working set for the auxiliary initial QP in
-		 *  accordance with the user specifications
-		 *  (assumes that member AX has already been initialised!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_OBTAINING_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,						/**< Optimal primal solution vector.
-																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const real_t* const yOpt,						/**< Optimal dual solution vector.
-																								 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
-												const Constraints* const guessedConstraints,	/**< Guessed working set for solution (xOpt,yOpt). */
-												Bounds* auxiliaryBounds,						/**< Input: Allocated bound object. \n
-																								 *	 Ouput: Working set of constraints for auxiliary QP. */
-												Constraints* auxiliaryConstraints				/**< Input: Allocated bound object. \n
-																								 *	 Ouput: Working set for auxiliary QP. */
-												) const;
-
-		/** Setups bound and constraints data structures according to auxiliaryBounds/Constraints.
-		 *  (If the working set shall be setup afresh, make sure that
-		 *  bounds and constraints data structure have been resetted
-		 *  and the TQ factorisation has been initialised!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUP_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNOWN BUG */
-		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
-												const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-												BooleanType setupAfresh							/**< Flag indicating if given working set shall be
-																								 *    setup afresh or by updating the current one. */
-												);
-
-		/** Setups the optimal primal/dual solution of the auxiliary initial QP.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
-												const real_t* const yOpt			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are set to zero. */
-												);
-
-		/** Setups gradient of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPgradient( );
-
-		/** Setups (constraints') bounds of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNKNOWN BUG */
-		returnValue setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,			/**< Working set of bounds for auxiliary QP. */
-											const Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
-											BooleanType useRelaxation						/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
-											);
-
-
-		/** Adds a constraint to active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDCONSTRAINT_FAILED \n
-					RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
-					RET_ENSURELI_FAILED */
-		returnValue addConstraint(	int number,					/**< Number of constraint to be added to active set. */
-									SubjectToStatus C_status,	/**< Status of new active constraint. */
-									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									);
-
-		/** Checks if new active constraint to be added is linearly dependent from
-		 *	from row of the active constraints matrix.
-		 *	\return	 RET_LINEARLY_DEPENDENT \n
-		 			 RET_LINEARLY_INDEPENDENT \n
-					 RET_INDEXLIST_CORRUPTED */
-		returnValue addConstraint_checkLI(	int number			/**< Number of constraint to be added to active set. */
-											);
-
-		/** Ensures linear independence of constraint matrix when a new constraint is added.
-		 * 	To this end a bound or constraint is removed simultaneously if necessary.
-		 *	\return	 SUCCESSFUL_RETURN \n
-		 			 RET_LI_RESOLVED \n
-					 RET_ENSURELI_FAILED \n
-					 RET_ENSURELI_FAILED_TQ \n
-					 RET_ENSURELI_FAILED_NOINDEX \n
-					 RET_REMOVE_FROM_ACTIVESET */
-		returnValue addConstraint_ensureLI(	int number,					/**< Number of constraint to be added to active set. */
-											SubjectToStatus C_status	/**< Status of new active bound. */
-											);
-
-		/** Adds a bound to active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDBOUND_FAILED \n
-					RET_ADDBOUND_FAILED_INFEASIBILITY \n
-					RET_ENSURELI_FAILED */
-		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
-								SubjectToStatus B_status,	/**< Status of new active bound. */
-								BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-								);
-
-		/** Checks if new active bound to be added is linearly dependent from
-		 *	from row of the active constraints matrix.
-		 *	\return	 RET_LINEARLY_DEPENDENT \n
-		 			 RET_LINEARLY_INDEPENDENT */
-		returnValue addBound_checkLI(	int number			/**< Number of bound to be added to active set. */
-										);
-
-		/** Ensures linear independence of constraint matrix when a new bound is added.
-		 *	To this end a bound or constraint is removed simultaneously if necessary.
-		 *	\return	 SUCCESSFUL_RETURN \n
-		 			 RET_LI_RESOLVED \n
-					 RET_ENSURELI_FAILED \n
-					 RET_ENSURELI_FAILED_TQ \n
-					 RET_ENSURELI_FAILED_NOINDEX \n
-					 RET_REMOVE_FROM_ACTIVESET */
-		returnValue addBound_ensureLI(	int number,					/**< Number of bound to be added to active set. */
-										SubjectToStatus B_status	/**< Status of new active bound. */
-										);
-
-		/** Removes a constraint from active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_CONSTRAINT_NOT_ACTIVE \n
-					RET_REMOVECONSTRAINT_FAILED \n
-					RET_HESSIAN_NOT_SPD */
-		returnValue removeConstraint(	int number,					/**< Number of constraint to be removed from active set. */
-										BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-										);
-
-		/** Removes a bounds from active set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_BOUND_NOT_ACTIVE \n
-					RET_HESSIAN_NOT_SPD \n
-					RET_REMOVEBOUND_FAILED */
-		returnValue removeBound(	int number,					/**< Number of bound to be removed from active set. */
-									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									);
-
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,	/**< Right hand side vector. */
-								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-								real_t* const a 		/**< Output: Solution vector */
-								);
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
-		 *  Special variant for the case that this function is called from within "removeBound()".
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,		/**< Right hand side vector. */
-								BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
-								BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
-								real_t* const a 			/**< Output: Solution vector */
-								);
-
-
-		/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_DIV_BY_ZERO */
-		returnValue backsolveT(	const real_t* const b,	/**< Right hand side vector. */
-								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-								real_t* const a 		/**< Output: Solution vector */
-								);
-
-
-		/** Determines step direction of the shift of the QP data.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue hotstart_determineDataShift(const int* const FX_idx, 	/**< Index array of fixed variables. */
-												const int* const AC_idx, 	/**< Index array of active constraints. */
-												const real_t* const g_new,	/**< New gradient vector. */
-												const real_t* const lbA_new,/**< New lower constraints' bounds. */
-												const real_t* const ubA_new,/**< New upper constraints' bounds. */
-												const real_t* const lb_new,	/**< New lower bounds. */
-												const real_t* const ub_new,	/**< New upper bounds. */
-												real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
-												real_t* const delta_lbA,	/**< Output: Step direction of lower constraints' bounds. */
-												real_t* const delta_ubA,	/**< Output: Step direction of upper constraints' bounds. */
-												real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
-												real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
-												BooleanType& Delta_bC_isZero,/**< Output: Indicates if active constraints' bounds are to be shifted. */
-												BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
-												);
-
-		/** Determines step direction of the homotopy path.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_STEPDIRECTION_FAILED_TQ \n
-					RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue hotstart_determineStepDirection(const int* const FR_idx, 		/**< Index array of free variables. */
-													const int* const FX_idx, 		/**< Index array of fixed variables. */
-													const int* const AC_idx, 		/**< Index array of active constraints. */
-													const real_t* const delta_g,	/**< Step direction of gradient vector. */
-													const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
-													const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
-													const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-													const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-													BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
-													BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
-													real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
-													real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
-													real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
-													real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
-													);
-
-		/** Determines the maximum possible step length along the homotopy path.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue hotstart_determineStepLength(	const int* const FR_idx, 			/**< Index array of free variables. */
-													const int* const FX_idx, 			/**< Index array of fixed variables. */
-													const int* const AC_idx, 			/**< Index array of active constraints. */
-													const int* const IAC_idx, 			/**< Index array of inactive constraints. */
-													const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
-													const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
-													const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
-													const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
-													const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
-													const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
-													const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
-													const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
-													real_t* const delta_Ax,				/**< Output: Step in vector Ax. */
-													int& BC_idx, 						/**< Output: Index of blocking constraint. */
-													SubjectToStatus& BC_status,			/**< Output: Status of blocking constraint. */
-													BooleanType& BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
-													);
-
-		/** Performs a step along the homotopy path (and updates active set).
-		 *	\return  SUCCESSFUL_RETURN \n
-		 			 RET_OPTIMAL_SOLUTION_FOUND \n
-		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
-					 RET_ADD_TO_ACTIVESET_FAILED \n
-					 RET_QP_INFEASIBLE */
-		returnValue hotstart_performStep(	const int* const FR_idx, 			/**< Index array of free variables. */
-											const int* const FX_idx, 			/**< Index array of fixed variables. */
-											const int* const AC_idx, 			/**< Index array of active constraints. */
-											const int* const IAC_idx, 			/**< Index array of inactive constraints. */
-											const real_t* const delta_g,	 	/**< Step direction of gradient vector. */
-											const real_t* const delta_lbA,	 	/**< Step direction of lower constraints' bounds. */
-											const real_t* const delta_ubA,	 	/**< Step direction of upper constraints' bounds. */
-											const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
-											const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
-											const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
-											const real_t* const delta_xFR,	 	/**< Primal homotopy step direction of free variables. */
-											const real_t* const delta_yAC,	 	/**< Dual homotopy step direction of active constraints' multiplier. */
-											const real_t* const delta_yFX, 		/**< Dual homotopy step direction of fixed variables' multiplier. */
-											const real_t* const delta_Ax, 		/**< Step in vector Ax. */
-											int BC_idx, 						/**< Index of blocking constraint. */
-											SubjectToStatus BC_status,			/**< Status of blocking constraint. */
-											BooleanType BC_isBound 				/**< Indicates if blocking constraint is a bound. */
-											);
-
-
-		/** Checks if lower/upper (constraints') bounds remain consistent
-		 *  (i.e. if lb <= ub and lbA <= ubA ) during the current step.
-		 *	\return BT_TRUE iff (constraints") bounds remain consistent
-		 */
-		BooleanType areBoundsConsistent(	const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-											const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-											const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
-											const real_t* const delta_ubA	/**< Step direction of upper constraints' bounds. */
-											) const;
-
-
-		/** Setups internal QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. */
-									const real_t* const _R, 	/**< Cholesky factorization of the Hessian matrix. */
-									const real_t* const _g, 	/**< Gradient vector. */
-									const real_t* const _A,  	/**< Constraint matrix. */
-									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
-																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
-									);
-
-
-		#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-
-		/** Prints concise information on the current iteration.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printIteration(	int iteration,				/**< Number of current iteration. */
-									int BC_idx, 				/**< Index of blocking constraint. */
-									SubjectToStatus BC_status,	/**< Status of blocking constraint. */
-									BooleanType BC_isBound 		/**< Indicates if blocking constraint is a bound. */
-									);
-
-		/** Prints concise information on the current iteration.
-		 *  NOTE: ONLY DEFINED FOR SUPPRESSING A COMPILER WARNING!!
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printIteration(	int iteration,				/**< Number of current iteration. */
-									int BC_idx, 				/**< Index of blocking bound. */
-									SubjectToStatus BC_status	/**< Status of blocking bound. */
-									);
-
-		#endif  /* PC_DEBUG */
-
-
-		/** Determines the maximum violation of the KKT optimality conditions
-		 *  of the current iterate within the QProblem object.
-		 *	\return SUCCESSFUL_RETURN \n
-		 * 			RET_INACCURATE_SOLUTION \n
-		 * 			RET_NO_SOLUTION */
-		returnValue checkKKTconditions( );
-
-
-		/** Sets constraint matrix of the QP. \n
-			(Remark: Also internal vector Ax is recomputed!)
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setA(	const real_t* const A_new	/**< New constraint matrix (with correct dimension!). */
-									);
-
-		/** Changes single row of constraint matrix of the QP. \n
-			(Remark: Also correponding component of internal vector Ax is recomputed!)
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setA(	int number,					/**< Number of row to be changed. */
-									const real_t* const value	/**< New (number)th constraint (with correct dimension!). */
-									);
-
-
-		/** Sets constraints' lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setLBA(	const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of lower constraints' bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setLBA(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
-									);
-
-		/** Sets constraints' upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setUBA(	const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of upper constraints' bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setUBA(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
-									);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		real_t A[NCMAX_ALLOC*NVMAX];		/**< Constraint matrix. */
-		real_t lbA[NCMAX_ALLOC];			/**< Lower constraints' bound vector. */
-		real_t ubA[NCMAX_ALLOC];			/**< Upper constraints' bound vector. */
-
-		Constraints constraints;			/**< Data structure for problem's constraints. */
-
-		real_t T[NVMAX*NVMAX];				/**< Reverse triangular matrix, A = [0 T]*Q'. */
-		real_t Q[NVMAX*NVMAX];				/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
-		int sizeT;							/**< Matrix T is stored in a (sizeT x sizeT) array. */
-
-		real_t Ax[NCMAX_ALLOC];				/**< Stores the current product A*x (for increased efficiency only). */
-
-		CyclingManager cyclingManager;		/**< Data structure for storing (possible) cycling information (NOT YET IMPLEMENTED!). */
-};
-
-
-#include <QProblem.ipp>
-
-#endif	/* QPOASES_QPROBLEM_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/QProblemB.hpp b/phonelibs/qpoases/INCLUDE/QProblemB.hpp
deleted file mode 100644
index 49ace9ef9..000000000
--- a/phonelibs/qpoases/INCLUDE/QProblemB.hpp
+++ /dev/null
@@ -1,628 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/QProblemB.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the QProblemB class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming
- *	for problems with (simple) bounds only.
- */
-
-
-
-#ifndef QPOASES_QPROBLEMB_HPP
-#define QPOASES_QPROBLEMB_HPP
-
-
-#include <Bounds.hpp>
-
-
-
-class SolutionAnalysis;
-
-/** Class for setting up and solving quadratic programs with (simple) bounds only.
- *	The main feature is the possibily to use the newly developed online active set strategy
- *	for parametric quadratic programming.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class QProblemB
-{
-	/* allow SolutionAnalysis class to access private members */
-	friend class SolutionAnalysis;
-	
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		QProblemB( );
-
-		/** Constructor which takes the QP dimension only. */
-		QProblemB(	int _nV						/**< Number of variables. */
-					);
-
-		/** Copy constructor (deep copy). */
-		QProblemB(	const QProblemB& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~QProblemB( );
-
-		/** Assignment operator (deep copy). */
-		QProblemB& operator=(	const QProblemB& rhs	/**< Rhs object. */
-								);
-
-
-		/** Clears all data structures of QProblemB except for QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_RESET_FAILED */
-		returnValue reset( );
-
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue init(	const real_t* const _H, 		/**< Hessian matrix. */
-							const real_t* const _g,			/**< Gradient vector. */
-							const real_t* const _lb,		/**< Lower bounds (on variables). \n
-																If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,		/**< Upper bounds (on variables). \n
-																If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 						/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-																Output: Number of performed working set recalculations. */
-							const real_t* const yOpt = 0,	/**< Initial guess for dual solution vector. */
-				 			real_t* const cputime = 0		/**< Output: CPU time required to initialise QP. */
-							);
-
-
-		/** Initialises a QProblemB with given QP data and solves it
-		 *	using an initial homotopy with empty working set (at most nWSR iterations).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue init(	const real_t* const _H, 		/**< Hessian matrix. */
-							const real_t* const _R, 		/**< Cholesky factorization of the Hessian matrix. */
-							const real_t* const _g,			/**< Gradient vector. */
-							const real_t* const _lb,		/**< Lower bounds (on variables). \n
-																If no lower bounds exist, a NULL pointer can be passed. */
-							const real_t* const _ub,		/**< Upper bounds (on variables). \n
-																If no upper bounds exist, a NULL pointer can be passed. */
-							int& nWSR, 						/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
-																Output: Number of performed working set recalculations. */
-							const real_t* const yOpt = 0,	/**< Initial guess for dual solution vector. */
-				 			real_t* const cputime = 0		/**< Output: CPU time required to initialise QP. */
-							);
-
-
-		/** Solves an initialised QProblemB using online active set strategy.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_MAX_NWSR_REACHED \n
-					RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
-					RET_HOTSTART_FAILED \n
-					RET_SHIFT_DETERMINATION_FAILED \n
-					RET_STEPDIRECTION_DETERMINATION_FAILED \n
-					RET_STEPLENGTH_DETERMINATION_FAILED \n
-					RET_HOMOTOPY_STEP_FAILED \n
-					RET_HOTSTART_STOPPED_INFEASIBILITY \n
-					RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
-					RET_INACCURATE_SOLUTION \n
-		 			RET_NO_SOLUTION */
-		returnValue hotstart(	const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
-								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
-													 			 If no lower bounds exist, a NULL pointer can be passed. */
-								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
-													 			 If no upper bounds exist, a NULL pointer can be passed. */
-								int& nWSR,					/**< Input: Maximum number of working set recalculations; \n
-																 Output: Number of performed working set recalculations. */
-								real_t* const cputime		/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
-								);
-
-
-		/** Returns Hessian matrix of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getH(	real_t* const _H	/**< Array of appropriate dimension for copying Hessian matrix.*/
-									) const;
-
-		/** Returns gradient vector of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getG(	real_t* const _g	/**< Array of appropriate dimension for copying gradient vector.*/
-									) const;
-
-		/** Returns lower bound vector of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getLB(	real_t* const _lb	/**< Array of appropriate dimension for copying lower bound vector.*/
-									) const;
-
-		/** Returns single entry of lower bound vector of the QP.
-		  *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue getLB(	int number,		/**< Number of entry to be returned. */
-									real_t& value	/**< Output: lb[number].*/
-									) const;
-
-		/** Returns upper bound vector of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getUB(	real_t* const _ub	/**< Array of appropriate dimension for copying upper bound vector.*/
-									) const;
-
-		/** Returns single entry of upper bound vector of the QP.
-		  *	\return SUCCESSFUL_RETURN \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue getUB(	int number,		/**< Number of entry to be returned. */
-									real_t& value	/**< Output: ub[number].*/
-									) const;
-
-
-		/** Returns current bounds object of the QP (deep copy).
-		  *	\return SUCCESSFUL_RETURN */
-		inline returnValue getBounds(	Bounds* const _bounds	/** Output: Bounds object. */
-										) const;
-
-
-		/** Returns the number of variables.
-		 *	\return Number of variables. */
-		inline int getNV( ) const;
-
-		/** Returns the number of free variables.
-		 *	\return Number of free variables. */
-		inline int getNFR( );
-
-		/** Returns the number of fixed variables.
-		 *	\return Number of fixed variables. */
-		inline int getNFX( );
-
-		/** Returns the number of implicitly fixed variables.
-		 *	\return Number of implicitly fixed variables. */
-		inline int getNFV( ) const;
-
-		/** Returns the dimension of null space.
-		 *	\return Dimension of null space. */
-		int getNZ( );
-
-
-		/** Returns the optimal objective function value.
-		 *	\return finite value: Optimal objective function value (QP was solved) \n
-		 			+infinity:	  QP was not yet solved */
-		real_t getObjVal( ) const;
-
-		/** Returns the objective function value at an arbitrary point x.
-		 *	\return Objective function value at point x */
-		real_t getObjVal(	const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
-							) const;
-
-		/** Returns the primal solution vector.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		returnValue getPrimalSolution(	real_t* const xOpt			/**< Output: Primal solution vector (if QP has been solved). */
-										) const;
-
-		/** Returns the dual solution vector.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_QP_NOT_SOLVED */
-		returnValue getDualSolution(	real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
-										) const;
-
-
-		/** Returns status of the solution process.
-		 *	\return Status of solution process. */
-		inline QProblemStatus getStatus( ) const;
-
-
-		/** Returns if the QProblem object is initialised.
-		 *	\return BT_TRUE:  QProblemB initialised \n
-		 			BT_FALSE: QProblemB not initialised */
-		inline BooleanType isInitialised( ) const;
-
-		/** Returns if the QP has been solved.
-		 *	\return BT_TRUE:  QProblemB solved \n
-		 			BT_FALSE: QProblemB not solved */
-		inline BooleanType isSolved( ) const;
-
-		/** Returns if the QP is infeasible.
-		 *	\return BT_TRUE:  QP infeasible \n
-		 			BT_FALSE: QP feasible (or not known to be infeasible!) */
-		inline BooleanType isInfeasible( ) const;
-
-		/** Returns if the QP is unbounded.
-		 *	\return BT_TRUE:  QP unbounded \n
-		 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
-		inline BooleanType isUnbounded( ) const;
-
-
-		/** Returns the print level.
-		 *	\return Print level. */
-		inline PrintLevel getPrintLevel( ) const;
-
-		/** Changes the print level.
- 		 *	\return SUCCESSFUL_RETURN */
-		returnValue setPrintLevel(	PrintLevel _printlevel	/**< New print level. */
-									);
-
-
-		/** Returns Hessian type flag (type is not determined due to this call!).
-		 *	\return Hessian type. */
-		inline HessianType getHessianType( ) const;
-
-		/** Changes the print level.
- 		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setHessianType(	HessianType _hessianType /**< New Hessian type. */
-											);
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Checks if Hessian happens to be the identity matrix,
-		 *  and sets corresponding status flag (otherwise the flag remains unaltered!).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue checkForIdentityHessian( );
-
-		/** Determines type of constraints and bounds (i.e. implicitly fixed, unbounded etc.).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUPSUBJECTTOTYPE_FAILED */
-		returnValue setupSubjectToType( );
-
-		/** Computes the Cholesky decomposition R of the (simply projected) Hessian (i.e. R^T*R = Z^T*H*Z).
-		 *  It only works in the case where Z is a simple projection matrix!
-		 *	\return SUCCESSFUL_RETURN \n
-		 *			RET_INDEXLIST_CORRUPTED */
-		returnValue setupCholeskyDecomposition( );
-
-
-		/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
-		 *  A guess for its primal/dual optimal solution vectors and the corresponding
-		 *  optimal working set can be provided.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INIT_FAILED \n
-					RET_INIT_FAILED_CHOLESKY \n
-					RET_INIT_FAILED_HOTSTART \n
-					RET_INIT_FAILED_INFEASIBILITY \n
-					RET_INIT_FAILED_UNBOUNDEDNESS \n
-					RET_MAX_NWSR_REACHED */
-		returnValue solveInitialQP(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																		 *	 A NULL pointer can be passed. */
-									const real_t* const yOpt,			/**< Optimal dual solution vector.
-																		 *	 A NULL pointer can be passed. */
-									const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt).
-																		 *	 A NULL pointer can be passed. */
-									int& nWSR, 							/**< Input: Maximum number of working set recalculations; \n
-																 		 *	 Output: Number of performed working set recalculations. */
-									real_t* const cputime				/**< Output: CPU time required to solve QP (or to perform nWSR iterations). */
-									);
-
-
-		/** Obtains the desired working set for the auxiliary initial QP in
-		 *  accordance with the user specifications
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_OBTAINING_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS */
-		returnValue obtainAuxiliaryWorkingSet(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const real_t* const yOpt,			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
-												const Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
-												Bounds* auxiliaryBounds				/**< Input: Allocated bound object. \n
-																					 *	 Ouput: Working set for auxiliary QP. */
-												) const;
-
-		/** Setups bound data structure according to auxiliaryBounds.
-		 *  (If the working set shall be setup afresh, make sure that
-		 *  bounds data structure has been resetted!)
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_SETUP_WORKINGSET_FAILED \n
-					RET_INVALID_ARGUMENTS \n
-					RET_UNKNOWN BUG */
-		returnValue setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,	/**< Working set for auxiliary QP. */
-												BooleanType setupAfresh					/**< Flag indicating if given working set shall be
-																						 *    setup afresh or by updating the current one. */
-												);
-
-		/** Setups the optimal primal/dual solution of the auxiliary initial QP.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPsolution(	const real_t* const xOpt,			/**< Optimal primal solution vector.
-																				 	*	 If a NULL pointer is passed, all entries are set to zero. */
-												const real_t* const yOpt			/**< Optimal dual solution vector.
-																					 *	 If a NULL pointer is passed, all entries are set to zero. */
-												);
-
-		/** Setups gradient of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue setupAuxiliaryQPgradient( );
-
-		/** Setups bounds of the auxiliary initial QP for given
-		 *  optimal primal/dual solution and given initial working set
-		 *  (assumes that members X, Y and BOUNDS have already been initialised!).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_UNKNOWN BUG */
-		returnValue setupAuxiliaryQPbounds( BooleanType useRelaxation	/**< Flag indicating if inactive bounds shall be relaxed. */
-											);
-
-
-		/** Adds a bound to active set (specialised version for the case where no constraints exist).
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDBOUND_FAILED */
-		returnValue addBound(	int number,					/**< Number of bound to be added to active set. */
-								SubjectToStatus B_status,	/**< Status of new active bound. */
-								BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-								);
-
-		/** Removes a bounds from active set (specialised version for the case where no constraints exist).
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_HESSIAN_NOT_SPD \n
-					RET_REMOVEBOUND_FAILED */
-		returnValue removeBound(	int number,					/**< Number of bound to be removed from active set. */
-									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
-									);
-
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,	/**< Right hand side vector. */
-								BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
-								real_t* const a 		/**< Output: Solution vector */
-								);
-
-		/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
-		 *  Special variant for the case that this function is called from within "removeBound()".
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_DIV_BY_ZERO */
-		returnValue backsolveR(	const real_t* const b,		/**< Right hand side vector. */
-								BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
-								BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
-								real_t* const a 			/**< Output: Solution vector */
-								);
-
-
-		/** Determines step direction of the shift of the QP data.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue hotstart_determineDataShift(const int* const FX_idx, 	/**< Index array of fixed variables. */
-												const real_t* const g_new,	/**< New gradient vector. */
-												const real_t* const lb_new,	/**< New lower bounds. */
-												const real_t* const ub_new,	/**< New upper bounds. */
-												real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
-												real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
-												real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
-												BooleanType& Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
-												);
-
-
-		/** Checks if lower/upper bounds remain consistent
-		 *  (i.e. if lb <= ub) during the current step.
-		 *	\return BT_TRUE iff bounds remain consistent
-		 */
-		BooleanType areBoundsConsistent(	const real_t* const delta_lb,		/**< Step direction of lower bounds. */
-											const real_t* const delta_ub		/**< Step direction of upper bounds. */
-											) const;
-
-
-		/** Setups internal QP data.
-		 *	\return SUCCESSFUL_RETURN \n
-					RET_INVALID_ARGUMENTS */
-		returnValue setupQPdata(	const real_t* const _H, 	/**< Hessian matrix. */
-									const real_t* const _R, 	/**< Cholesky factorization of the Hessian matrix. */
-									const real_t* const _g,		/**< Gradient vector. */
-									const real_t* const _lb,	/**< Lower bounds (on variables). \n
-																	 If no lower bounds exist, a NULL pointer can be passed. */
-									const real_t* const _ub		/**< Upper bounds (on variables). \n
-																	 If no upper bounds exist, a NULL pointer can be passed. */
-									);
-
-
-		/** Sets Hessian matrix of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setH(	const real_t* const H_new	/**< New Hessian matrix (with correct dimension!). */
-									);
-
-		/** Changes gradient vector of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setG(	const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
-									);
-
-		/** Changes lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setLB(	const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of lower bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setLB(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of lower bound vector. */
-									);
-
-		/** Changes upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN */
-		inline returnValue setUB(	const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
-									);
-
-		/** Changes single entry of upper bound vector of the QP.
-		 *	\return SUCCESSFUL_RETURN  \n
-					RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setUB(	int number,		/**< Number of entry to be changed. */
-									real_t value	/**< New value for entry of upper bound vector. */
-									);
-
-
-		/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
-		 *	\return SUCCESSFUL_RETURN */
-		inline void computeGivens(	real_t xold,	/**< Matrix entry to be normalised. */
-									real_t yold,	/**< Matrix entry to be annihilated. */
-									real_t& xnew,	/**< Output: Normalised matrix entry. */
-									real_t& ynew,	/**< Output: Annihilated matrix entry. */
-									real_t& c,		/**< Output: Cosine entry of Givens matrix. */
-									real_t& s 		/**< Output: Sine entry of Givens matrix. */
-									) const;
-
-		/** Applies Givens matrix determined by c and s (cf. computeGivens).
-		 *	\return SUCCESSFUL_RETURN */
-		inline void applyGivens(	real_t c,		/**< Cosine entry of Givens matrix. */
-									real_t s,		/**< Sine entry of Givens matrix. */
-									real_t xold,	/**< Matrix entry to be transformed corresponding to
-													 *	 the normalised entry of the original matrix. */
-									real_t yold, 	/**< Matrix entry to be transformed corresponding to
-													 *	 the annihilated entry of the original matrix. */
-									real_t& xnew,	/**< Output: Transformed matrix entry corresponding to
-													 *	 the normalised entry of the original matrix. */
-									real_t& ynew	/**< Output: Transformed matrix entry corresponding to
-													 *	 the annihilated entry of the original matrix. */
-									) const;
-
-
-	/*
-	 *	PRIVATE MEMBER FUNCTIONS
-	 */
-	private:
-		/** Determines step direction of the homotopy path.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_STEPDIRECTION_FAILED_CHOLESKY */
-		returnValue hotstart_determineStepDirection(const int* const FR_idx, 		/**< Index array of free variables. */
-													const int* const FX_idx, 		/**< Index array of fixed variables. */
-													const real_t* const delta_g,	/**< Step direction of gradient vector. */
-													const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-													const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-													BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
-													real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
-													real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
-													real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
-													);
-
-		/** Determines the maximum possible step length along the homotopy path.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue hotstart_determineStepLength(	const int* const FR_idx, 		/**< Index array of free variables. */
-													const int* const FX_idx, 		/**< Index array of fixed variables. */
-													const real_t* const delta_lb,	/**< Step direction of lower bounds. */
-													const real_t* const delta_ub,	/**< Step direction of upper bounds. */
-													const real_t* const delta_xFR,	/**< Primal homotopy step direction of free variables. */
-													const real_t* const delta_yFX,	/**< Dual homotopy step direction of fixed variables' multiplier. */
-													int& BC_idx, 					/**< Output: Index of blocking constraint. */
-													SubjectToStatus& BC_status		/**< Output: Status of blocking constraint. */
-													);
-
-		/** Performs a step along the homotopy path (and updates active set).
-		 *	\return  SUCCESSFUL_RETURN \n
-		 			 RET_OPTIMAL_SOLUTION_FOUND \n
-		 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
-					 RET_ADD_TO_ACTIVESET_FAILED \n
-					 RET_QP_INFEASIBLE */
-		returnValue hotstart_performStep(	const int* const FR_idx, 			/**< Index array of free variables. */
-											const int* const FX_idx, 			/**< Index array of fixed variables. */
-											const real_t* const delta_g,	 	/**< Step direction of gradient vector. */
-											const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
-											const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
-											const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
-											const real_t* const delta_xFR,	 	/**< Primal homotopy step direction of free variables. */
-											const real_t* const delta_yFX, 		/**< Dual homotopy step direction of fixed variables' multiplier. */
-											int BC_idx, 						/**< Index of blocking constraint. */
-											SubjectToStatus BC_status 			/**< Status of blocking constraint. */
-											);
-
-
-		#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-
-		/** Prints concise information on the current iteration.
-		 *	\return  SUCCESSFUL_RETURN \n */
-		returnValue printIteration(	int iteration,				/**< Number of current iteration. */
-									int BC_idx, 				/**< Index of blocking bound. */
-									SubjectToStatus BC_status	/**< Status of blocking bound. */
-									);
-
-		#endif  /* PC_DEBUG */
-
-
-		/** Determines the maximum violation of the KKT optimality conditions
-		 *  of the current iterate within the QProblemB object.
-		 *	\return SUCCESSFUL_RETURN \n
-		 * 			RET_INACCURATE_SOLUTION \n
-		 * 			RET_NO_SOLUTION */
-		returnValue checkKKTconditions( );
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		real_t H[NVMAX*NVMAX];		/**< Hessian matrix. */
-		BooleanType hasHessian;		/**< Flag indicating whether H contains Hessian or corresponding Cholesky factor R; \sa init. */
-
-		real_t g[NVMAX];			/**< Gradient. */
-		real_t lb[NVMAX];			/**< Lower bound vector (on variables). */
-		real_t ub[NVMAX];			/**< Upper bound vector (on variables). */
-
-		Bounds bounds;				/**< Data structure for problem's bounds. */
-
-		real_t R[NVMAX*NVMAX];		/**< Cholesky decomposition of H (i.e. H = R^T*R). */
-		BooleanType hasCholesky;	/**< Flag indicating whether Cholesky decomposition has already been setup. */
-
-		real_t x[NVMAX];			/**< Primal solution vector. */
-		real_t y[NVMAX+NCMAX];		/**< Dual solution vector. */
-
-		real_t tau;					/**< Last homotopy step length. */
-
-		QProblemStatus status;		/**< Current status of the solution process. */
-
-		BooleanType infeasible;		/**< QP infeasible? */
-		BooleanType unbounded;		/**< QP unbounded? */
-
-		HessianType hessianType;	/**< Type of Hessian matrix. */
-
-		PrintLevel printlevel;		/**< Print level. */
-
-		int count;					/**< Counts the number of hotstart function calls (internal usage only!). */
-};
-
-
-#include <QProblemB.ipp>
-
-#endif	/* QPOASES_QPROBLEMB_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/SubjectTo.hpp b/phonelibs/qpoases/INCLUDE/SubjectTo.hpp
deleted file mode 100644
index e07bf0421..000000000
--- a/phonelibs/qpoases/INCLUDE/SubjectTo.hpp
+++ /dev/null
@@ -1,178 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/SubjectTo.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of the SubjectTo class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#ifndef QPOASES_SUBJECTTO_HPP
-#define QPOASES_SUBJECTTO_HPP
-
-
-#include <Indexlist.hpp>
-
-
-
-/** This class manages working sets of constraints and bounds by storing
- *	index sets and other status information.
- *
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- */
-class SubjectTo
-{
-	/*
-	 *	PUBLIC MEMBER FUNCTIONS
-	 */
-	public:
-		/** Default constructor. */
-		SubjectTo( );
-
-		/** Copy constructor (deep copy). */
-		SubjectTo(	const SubjectTo& rhs	/**< Rhs object. */
-					);
-
-		/** Destructor. */
-		~SubjectTo( );
-
-		/** Assignment operator (deep copy). */
-		SubjectTo& operator=(	const SubjectTo& rhs	/**< Rhs object. */
-								);
-
-
-		/** Pseudo-constructor takes the number of constraints or bounds.
-		 *	\return SUCCESSFUL_RETURN */
-		returnValue init(	int n 	/**< Number of constraints or bounds. */
-							);
-
-
-		/** Returns type of (constraints') bound.
-		 *	\return Type of (constraints') bound \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline SubjectToType getType(	int i		/**< Number of (constraints') bound. */
-										) const ;
-
-		/** Returns status of (constraints') bound.
-		 *	\return Status of (constraints') bound \n
-		 			ST_UNDEFINED */
-		inline SubjectToStatus getStatus(	int i		/**< Number of (constraints') bound. */
-											) const;
-
-
-		/** Sets type of (constraints') bound.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setType(	int i,				/**< Number of (constraints') bound. */
-									SubjectToType value	/**< Type of (constraints') bound. */
-									);
-
-		/** Sets status of (constraints') bound.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_INDEX_OUT_OF_BOUNDS */
-		inline returnValue setStatus(	int i,					/**< Number of (constraints') bound. */
-										SubjectToStatus value	/**< Status of (constraints') bound. */
-										);
-
-
-		/** Sets status of lower (constraints') bounds. */
-		inline void setNoLower(	BooleanType _status		/**< Status of lower (constraints') bounds. */
-								);
-
-		/** Sets status of upper (constraints') bounds. */
-		inline void setNoUpper(	BooleanType _status		/**< Status of upper (constraints') bounds. */
-								);
-
-
-		/** Returns status of lower (constraints') bounds.
-		 *	\return BT_TRUE if there is no lower (constraints') bound on any variable. */
-		inline BooleanType isNoLower( ) const;
-
-		/** Returns status of upper bounds.
-		 *	\return BT_TRUE if there is no upper (constraints') bound on any variable. */
-		inline BooleanType isNoUpper( ) const;
-
-
-	/*
-	 *	PROTECTED MEMBER FUNCTIONS
-	 */
-	protected:
-		/** Adds the index of a new constraint or bound to index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_ADDINDEX_FAILED */
-		returnValue addIndex(	Indexlist* const indexlist,	/**< Index list to which the new index shall be added. */
-								int newnumber,				/**< Number of new constraint or bound. */
-								SubjectToStatus newstatus	/**< Status of new constraint or bound. */
-								);
-
-		/** Removes the index of a constraint or bound from index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_UNKNOWN_BUG */
-		returnValue removeIndex(	Indexlist* const indexlist,	/**< Index list from which the new index shall be removed. */
-									int removenumber			/**< Number of constraint or bound to be removed. */
-									);
-
-		/** Swaps the indices of two constraints or bounds within the index set.
-		 *	\return SUCCESSFUL_RETURN \n
-		 			RET_SWAPINDEX_FAILED */
-		returnValue swapIndex(	Indexlist* const indexlist,	/**< Index list in which the indices shold be swapped. */
-								int number1,				/**< Number of first constraint or bound. */
-								int number2					/**< Number of second constraint or bound. */
-								);
-
-
-	/*
-	 *	PROTECTED MEMBER VARIABLES
-	 */
-	protected:
-		SubjectToType type[NVMAX+NCMAX]; 		/**< Type of constraints/bounds. */
-		SubjectToStatus status[NVMAX+NCMAX];	/**< Status of constraints/bounds. */
-
-		BooleanType noLower;				 	/**< This flag indicates if there is no lower bound on any variable. */
-		BooleanType noUpper;	 				/**< This flag indicates if there is no upper bound on any variable. */
-
-
-	/*
-	 *	PRIVATE MEMBER VARIABLES
-	 */
-	private:
-		int size;
-};
-
-
-
-#include <SubjectTo.ipp>
-
-#endif	/* QPOASES_SUBJECTTO_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/Types.hpp b/phonelibs/qpoases/INCLUDE/Types.hpp
deleted file mode 100644
index 5b873ad07..000000000
--- a/phonelibs/qpoases/INCLUDE/Types.hpp
+++ /dev/null
@@ -1,131 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Types.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2008
- *
- *	Declaration of all non-built-in types (except for classes).
- */
-
-
-#ifndef QPOASES_TYPES_HPP
-#define QPOASES_TYPES_HPP
-
-
-
-/** Define real_t for facilitating switching between double and float. */
-// typedef double real_t;
-
-
-/** Summarises all possible logical values. */
-enum BooleanType
-{
-	BT_FALSE,					/**< Logical value for "false". */
-	BT_TRUE						/**< Logical value for "true". */
-};
-
-
-/** Summarises all possible print levels. Print levels are used to describe
- *	the desired amount of output during runtime of qpOASES. */
-enum PrintLevel
-{
-	PL_NONE,					/**< No output. */
-	PL_LOW,						/**< Print error messages only. */
-	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
-	PL_HIGH						/**< Print all messages with full details. */
-};
-
-
-/** Defines visibility status of a message. */
-enum VisibilityStatus
-{
-	VS_VISIBLE,					/**< Message visible. */
-	VS_HIDDEN					/**< Message not visible. */
-};
-
-
-/** Summarises all possible states of the (S)QProblem(B) object during the
-solution process of a QP sequence. */
-enum QProblemStatus
-{
-	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
-	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
-								 *   via an initial homotopy or after changing the QP matrices. */
-	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
-								 *   set strategy is performed. */
-	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
-	QPS_SOLVED					/**< The solution of the actual QP was found. */
-};
-
-
-/** Summarises all possible types of bounds and constraints. */
-enum SubjectToType
-{
-	ST_UNBOUNDED,				/**< Bound/constraint is unbounded. */
-	ST_BOUNDED,					/**< Bound/constraint is bounded but not fixed. */
-	ST_EQUALITY,				/**< Bound/constraint is fixed (implicit equality bound/constraint). */
-	ST_UNKNOWN					/**< Type of bound/constraint unknown. */
-};
-
-
-/** Summarises all possible states of bounds and constraints. */
-enum SubjectToStatus
-{
-	ST_INACTIVE,				/**< Bound/constraint is inactive. */
-	ST_LOWER,					/**< Bound/constraint is at its lower bound. */
-	ST_UPPER,					/**< Bound/constraint is at its upper bound. */
-	ST_UNDEFINED				/**< Status of bound/constraint undefined. */
-};
-
-
-/** Summarises all possible cycling states of bounds and constraints. */
-enum CyclingStatus
-{
-	CYC_NOT_INVOLVED,			/**< Bound/constraint is not involved in current cycling. */
-	CYC_PREV_ADDED,				/**< Bound/constraint has previously been added during the current cycling. */
-	CYC_PREV_REMOVED			/**< Bound/constraint has previously been removed during the current cycling. */
-};
-
-
-/** Summarises all possible types of the QP's Hessian matrix. */
-enum HessianType
-{
-	HST_SEMIDEF,				/**< Hessian is positive semi-definite. */
-	HST_POSDEF_NULLSPACE,		/**< Hessian is positive definite on null space of active bounds/constraints. */
-	HST_POSDEF,					/**< Hessian is (strictly) positive definite. */
-	HST_IDENTITY				/**< Hessian is identity matrix. */
-};
-
-
-
-#endif	/* QPOASES_TYPES_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/INCLUDE/Utils.hpp b/phonelibs/qpoases/INCLUDE/Utils.hpp
deleted file mode 100644
index a91ee78cc..000000000
--- a/phonelibs/qpoases/INCLUDE/Utils.hpp
+++ /dev/null
@@ -1,197 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file INCLUDE/Utils.hpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of global utility functions for working with qpOASES.
- */
-
-
-#ifndef QPOASES_UTILS_HPP
-#define QPOASES_UTILS_HPP
-
-
-#include <MessageHandling.hpp>
-
-
-#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-
-/** Prints a vector.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const v,	/**< Vector to be printed. */
-					int n					/**< Length of vector. */
-					);
-
-/** Prints a permuted vector.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const v,		/**< Vector to be printed. */
-					int n,						/**< Length of vector. */
-					const int* const V_idx		/**< Pemutation vector. */
-					);
-
-/** Prints a named vector.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const v,	/**< Vector to be printed. */
-					int n,					/**< Length of vector. */
-					const char* name		/** Name of vector. */
-					);
-
-/** Prints a matrix.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const M,	/**< Matrix to be printed. */
-					int nrow,				/**< Row number of matrix. */
-					int ncol				/**< Column number of matrix. */
-					);
-
-/** Prints a permuted matrix.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const M,		/**< Matrix to be printed. */
-					int nrow,					/**< Row number of matrix. */
-					int ncol	,				/**< Column number of matrix. */
-					const int* const ROW_idx,	/**< Row pemutation vector. */
-					const int* const COL_idx	/**< Column pemutation vector. */
-					);
-
-/** Prints a named matrix.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const real_t* const M,	/**< Matrix to be printed. */
-					int nrow,				/**< Row number of matrix. */
-					int ncol,				/**< Column number of matrix. */
-					const char* name		/** Name of matrix. */
-					);
-
-/** Prints an index array.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const int* const index,	/**< Index array to be printed. */
-					int n					/**< Length of index array. */
-					);
-
-/** Prints a named index array.
- * \return SUCCESSFUL_RETURN */
-returnValue print(	const int* const index,	/**< Index array to be printed. */
-					int n,					/**< Length of index array. */
-					const char* name		/**< Name of index array. */
-					);
-
-
-/** Prints a string to desired output target (useful also for MATLAB output!).
- * \return SUCCESSFUL_RETURN */
-returnValue myPrintf(	const char* s	/**< String to be written. */
-						);
-
-
-/** Prints qpOASES copyright notice.
- * \return SUCCESSFUL_RETURN */
-returnValue printCopyrightNotice( );
-
-
-/** Reads a real_t matrix from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	real_t* data,				/**< Matrix to be read from file. */
-							int nrow,					/**< Row number of matrix. */
-							int ncol,					/**< Column number of matrix. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-/** Reads a real_t vector from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	real_t* data,				/**< Vector to be read from file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-/** Reads an integer (column) vector from file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE \n
-		   RET_UNABLE_TO_READ_FILE */
-returnValue readFromFile(	int* data,					/**< Vector to be read from file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename	/**< Data file name. */
-							);
-
-
-/** Writes a real_t matrix into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE  */
-returnValue writeIntoFile(	const real_t* const data,	/**< Matrix to be written into file. */
-							int nrow,					/**< Row number of matrix. */
-							int ncol,					/**< Column number of matrix. */
-							const char* datafilename,	/**< Data file name. */
-							BooleanType append			/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-/** Writes a real_t vector into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE  */
-returnValue writeIntoFile(	const real_t* const data,	/**< Vector to be written into file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename,	/**< Data file name. */
-							BooleanType append			/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-/** Writes an integer (column) vector into a file.
- * \return SUCCESSFUL_RETURN \n
- 		   RET_UNABLE_TO_OPEN_FILE */
-returnValue writeIntoFile(	const int* const integer,	/**< Integer vector to be written into file. */
-							int n,						/**< Length of vector. */
-							const char* datafilename,	/**< Data file name. */
-							BooleanType append			/**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
-							);
-
-#endif  /* PC_DEBUG */
-
-
-/** Returns the current system time.
- * \return current system time */
-real_t getCPUtime( );
-
-
-/** Returns the Euclidean norm of a vector.
- * \return 0: successful */
-real_t getNorm(	const real_t* const v,	/**< Vector. */
-				int n					/**< Vector's dimension. */
-				);
-
-/** Returns the absolute value of a real_t.
- * \return Absolute value of a real_t */
-inline real_t getAbs(	real_t x		/**< Input argument. */
-						);
-
-
-
-#include <Utils.ipp>
-
-#endif	/* QPOASES_UTILS_HPP */
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/LICENSE.txt b/phonelibs/qpoases/LICENSE.txt
deleted file mode 100644
index 5ab7695ab..000000000
--- a/phonelibs/qpoases/LICENSE.txt
+++ /dev/null
@@ -1,504 +0,0 @@
-		  GNU LESSER GENERAL PUBLIC LICENSE
-		       Version 2.1, February 1999
-
- Copyright (C) 1991, 1999 Free Software Foundation, Inc.
- 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-[This is the first released version of the Lesser GPL.  It also counts
- as the successor of the GNU Library Public License, version 2, hence
- the version number 2.1.]
-
-			    Preamble
-
-  The licenses for most software are designed to take away your
-freedom to share and change it.  By contrast, the GNU General Public
-Licenses are intended to guarantee your freedom to share and change
-free software--to make sure the software is free for all its users.
-
-  This license, the Lesser General Public License, applies to some
-specially designated software packages--typically libraries--of the
-Free Software Foundation and other authors who decide to use it.  You
-can use it too, but we suggest you first think carefully about whether
-this license or the ordinary General Public License is the better
-strategy to use in any particular case, based on the explanations below.
-
-  When we speak of free software, we are referring to freedom of use,
-not price.  Our General Public Licenses are designed to make sure that
-you have the freedom to distribute copies of free software (and charge
-for this service if you wish); that you receive source code or can get
-it if you want it; that you can change the software and use pieces of
-it in new free programs; and that you are informed that you can do
-these things.
-
-  To protect your rights, we need to make restrictions that forbid
-distributors to deny you these rights or to ask you to surrender these
-rights.  These restrictions translate to certain responsibilities for
-you if you distribute copies of the library or if you modify it.
-
-  For example, if you distribute copies of the library, whether gratis
-or for a fee, you must give the recipients all the rights that we gave
-you.  You must make sure that they, too, receive or can get the source
-code.  If you link other code with the library, you must provide
-complete object files to the recipients, so that they can relink them
-with the library after making changes to the library and recompiling
-it.  And you must show them these terms so they know their rights.
-
-  We protect your rights with a two-step method: (1) we copyright the
-library, and (2) we offer you this license, which gives you legal
-permission to copy, distribute and/or modify the library.
-
-  To protect each distributor, we want to make it very clear that
-there is no warranty for the free library.  Also, if the library is
-modified by someone else and passed on, the recipients should know
-that what they have is not the original version, so that the original
-author's reputation will not be affected by problems that might be
-introduced by others.
-
-  Finally, software patents pose a constant threat to the existence of
-any free program.  We wish to make sure that a company cannot
-effectively restrict the users of a free program by obtaining a
-restrictive license from a patent holder.  Therefore, we insist that
-any patent license obtained for a version of the library must be
-consistent with the full freedom of use specified in this license.
-
-  Most GNU software, including some libraries, is covered by the
-ordinary GNU General Public License.  This license, the GNU Lesser
-General Public License, applies to certain designated libraries, and
-is quite different from the ordinary General Public License.  We use
-this license for certain libraries in order to permit linking those
-libraries into non-free programs.
-
-  When a program is linked with a library, whether statically or using
-a shared library, the combination of the two is legally speaking a
-combined work, a derivative of the original library.  The ordinary
-General Public License therefore permits such linking only if the
-entire combination fits its criteria of freedom.  The Lesser General
-Public License permits more lax criteria for linking other code with
-the library.
-
-  We call this license the "Lesser" General Public License because it
-does Less to protect the user's freedom than the ordinary General
-Public License.  It also provides other free software developers Less
-of an advantage over competing non-free programs.  These disadvantages
-are the reason we use the ordinary General Public License for many
-libraries.  However, the Lesser license provides advantages in certain
-special circumstances.
-
-  For example, on rare occasions, there may be a special need to
-encourage the widest possible use of a certain library, so that it becomes
-a de-facto standard.  To achieve this, non-free programs must be
-allowed to use the library.  A more frequent case is that a free
-library does the same job as widely used non-free libraries.  In this
-case, there is little to gain by limiting the free library to free
-software only, so we use the Lesser General Public License.
-
-  In other cases, permission to use a particular library in non-free
-programs enables a greater number of people to use a large body of
-free software.  For example, permission to use the GNU C Library in
-non-free programs enables many more people to use the whole GNU
-operating system, as well as its variant, the GNU/Linux operating
-system.
-
-  Although the Lesser General Public License is Less protective of the
-users' freedom, it does ensure that the user of a program that is
-linked with the Library has the freedom and the wherewithal to run
-that program using a modified version of the Library.
-
-  The precise terms and conditions for copying, distribution and
-modification follow.  Pay close attention to the difference between a
-"work based on the library" and a "work that uses the library".  The
-former contains code derived from the library, whereas the latter must
-be combined with the library in order to run.
-
-		  GNU LESSER GENERAL PUBLIC LICENSE
-   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
-
-  0. This License Agreement applies to any software library or other
-program which contains a notice placed by the copyright holder or
-other authorized party saying it may be distributed under the terms of
-this Lesser General Public License (also called "this License").
-Each licensee is addressed as "you".
-
-  A "library" means a collection of software functions and/or data
-prepared so as to be conveniently linked with application programs
-(which use some of those functions and data) to form executables.
-
-  The "Library", below, refers to any such software library or work
-which has been distributed under these terms.  A "work based on the
-Library" means either the Library or any derivative work under
-copyright law: that is to say, a work containing the Library or a
-portion of it, either verbatim or with modifications and/or translated
-straightforwardly into another language.  (Hereinafter, translation is
-included without limitation in the term "modification".)
-
-  "Source code" for a work means the preferred form of the work for
-making modifications to it.  For a library, complete source code means
-all the source code for all modules it contains, plus any associated
-interface definition files, plus the scripts used to control compilation
-and installation of the library.
-
-  Activities other than copying, distribution and modification are not
-covered by this License; they are outside its scope.  The act of
-running a program using the Library is not restricted, and output from
-such a program is covered only if its contents constitute a work based
-on the Library (independent of the use of the Library in a tool for
-writing it).  Whether that is true depends on what the Library does
-and what the program that uses the Library does.
-  
-  1. You may copy and distribute verbatim copies of the Library's
-complete source code as you receive it, in any medium, provided that
-you conspicuously and appropriately publish on each copy an
-appropriate copyright notice and disclaimer of warranty; keep intact
-all the notices that refer to this License and to the absence of any
-warranty; and distribute a copy of this License along with the
-Library.
-
-  You may charge a fee for the physical act of transferring a copy,
-and you may at your option offer warranty protection in exchange for a
-fee.
-
-  2. You may modify your copy or copies of the Library or any portion
-of it, thus forming a work based on the Library, and copy and
-distribute such modifications or work under the terms of Section 1
-above, provided that you also meet all of these conditions:
-
-    a) The modified work must itself be a software library.
-
-    b) You must cause the files modified to carry prominent notices
-    stating that you changed the files and the date of any change.
-
-    c) You must cause the whole of the work to be licensed at no
-    charge to all third parties under the terms of this License.
-
-    d) If a facility in the modified Library refers to a function or a
-    table of data to be supplied by an application program that uses
-    the facility, other than as an argument passed when the facility
-    is invoked, then you must make a good faith effort to ensure that,
-    in the event an application does not supply such function or
-    table, the facility still operates, and performs whatever part of
-    its purpose remains meaningful.
-
-    (For example, a function in a library to compute square roots has
-    a purpose that is entirely well-defined independent of the
-    application.  Therefore, Subsection 2d requires that any
-    application-supplied function or table used by this function must
-    be optional: if the application does not supply it, the square
-    root function must still compute square roots.)
-
-These requirements apply to the modified work as a whole.  If
-identifiable sections of that work are not derived from the Library,
-and can be reasonably considered independent and separate works in
-themselves, then this License, and its terms, do not apply to those
-sections when you distribute them as separate works.  But when you
-distribute the same sections as part of a whole which is a work based
-on the Library, the distribution of the whole must be on the terms of
-this License, whose permissions for other licensees extend to the
-entire whole, and thus to each and every part regardless of who wrote
-it.
-
-Thus, it is not the intent of this section to claim rights or contest
-your rights to work written entirely by you; rather, the intent is to
-exercise the right to control the distribution of derivative or
-collective works based on the Library.
-
-In addition, mere aggregation of another work not based on the Library
-with the Library (or with a work based on the Library) on a volume of
-a storage or distribution medium does not bring the other work under
-the scope of this License.
-
-  3. You may opt to apply the terms of the ordinary GNU General Public
-License instead of this License to a given copy of the Library.  To do
-this, you must alter all the notices that refer to this License, so
-that they refer to the ordinary GNU General Public License, version 2,
-instead of to this License.  (If a newer version than version 2 of the
-ordinary GNU General Public License has appeared, then you can specify
-that version instead if you wish.)  Do not make any other change in
-these notices.
-
-  Once this change is made in a given copy, it is irreversible for
-that copy, so the ordinary GNU General Public License applies to all
-subsequent copies and derivative works made from that copy.
-
-  This option is useful when you wish to copy part of the code of
-the Library into a program that is not a library.
-
-  4. You may copy and distribute the Library (or a portion or
-derivative of it, under Section 2) in object code or executable form
-under the terms of Sections 1 and 2 above provided that you accompany
-it with the complete corresponding machine-readable source code, which
-must be distributed under the terms of Sections 1 and 2 above on a
-medium customarily used for software interchange.
-
-  If distribution of object code is made by offering access to copy
-from a designated place, then offering equivalent access to copy the
-source code from the same place satisfies the requirement to
-distribute the source code, even though third parties are not
-compelled to copy the source along with the object code.
-
-  5. A program that contains no derivative of any portion of the
-Library, but is designed to work with the Library by being compiled or
-linked with it, is called a "work that uses the Library".  Such a
-work, in isolation, is not a derivative work of the Library, and
-therefore falls outside the scope of this License.
-
-  However, linking a "work that uses the Library" with the Library
-creates an executable that is a derivative of the Library (because it
-contains portions of the Library), rather than a "work that uses the
-library".  The executable is therefore covered by this License.
-Section 6 states terms for distribution of such executables.
-
-  When a "work that uses the Library" uses material from a header file
-that is part of the Library, the object code for the work may be a
-derivative work of the Library even though the source code is not.
-Whether this is true is especially significant if the work can be
-linked without the Library, or if the work is itself a library.  The
-threshold for this to be true is not precisely defined by law.
-
-  If such an object file uses only numerical parameters, data
-structure layouts and accessors, and small macros and small inline
-functions (ten lines or less in length), then the use of the object
-file is unrestricted, regardless of whether it is legally a derivative
-work.  (Executables containing this object code plus portions of the
-Library will still fall under Section 6.)
-
-  Otherwise, if the work is a derivative of the Library, you may
-distribute the object code for the work under the terms of Section 6.
-Any executables containing that work also fall under Section 6,
-whether or not they are linked directly with the Library itself.
-
-  6. As an exception to the Sections above, you may also combine or
-link a "work that uses the Library" with the Library to produce a
-work containing portions of the Library, and distribute that work
-under terms of your choice, provided that the terms permit
-modification of the work for the customer's own use and reverse
-engineering for debugging such modifications.
-
-  You must give prominent notice with each copy of the work that the
-Library is used in it and that the Library and its use are covered by
-this License.  You must supply a copy of this License.  If the work
-during execution displays copyright notices, you must include the
-copyright notice for the Library among them, as well as a reference
-directing the user to the copy of this License.  Also, you must do one
-of these things:
-
-    a) Accompany the work with the complete corresponding
-    machine-readable source code for the Library including whatever
-    changes were used in the work (which must be distributed under
-    Sections 1 and 2 above); and, if the work is an executable linked
-    with the Library, with the complete machine-readable "work that
-    uses the Library", as object code and/or source code, so that the
-    user can modify the Library and then relink to produce a modified
-    executable containing the modified Library.  (It is understood
-    that the user who changes the contents of definitions files in the
-    Library will not necessarily be able to recompile the application
-    to use the modified definitions.)
-
-    b) Use a suitable shared library mechanism for linking with the
-    Library.  A suitable mechanism is one that (1) uses at run time a
-    copy of the library already present on the user's computer system,
-    rather than copying library functions into the executable, and (2)
-    will operate properly with a modified version of the library, if
-    the user installs one, as long as the modified version is
-    interface-compatible with the version that the work was made with.
-
-    c) Accompany the work with a written offer, valid for at
-    least three years, to give the same user the materials
-    specified in Subsection 6a, above, for a charge no more
-    than the cost of performing this distribution.
-
-    d) If distribution of the work is made by offering access to copy
-    from a designated place, offer equivalent access to copy the above
-    specified materials from the same place.
-
-    e) Verify that the user has already received a copy of these
-    materials or that you have already sent this user a copy.
-
-  For an executable, the required form of the "work that uses the
-Library" must include any data and utility programs needed for
-reproducing the executable from it.  However, as a special exception,
-the materials to be distributed need not include anything that is
-normally distributed (in either source or binary form) with the major
-components (compiler, kernel, and so on) of the operating system on
-which the executable runs, unless that component itself accompanies
-the executable.
-
-  It may happen that this requirement contradicts the license
-restrictions of other proprietary libraries that do not normally
-accompany the operating system.  Such a contradiction means you cannot
-use both them and the Library together in an executable that you
-distribute.
-
-  7. You may place library facilities that are a work based on the
-Library side-by-side in a single library together with other library
-facilities not covered by this License, and distribute such a combined
-library, provided that the separate distribution of the work based on
-the Library and of the other library facilities is otherwise
-permitted, and provided that you do these two things:
-
-    a) Accompany the combined library with a copy of the same work
-    based on the Library, uncombined with any other library
-    facilities.  This must be distributed under the terms of the
-    Sections above.
-
-    b) Give prominent notice with the combined library of the fact
-    that part of it is a work based on the Library, and explaining
-    where to find the accompanying uncombined form of the same work.
-
-  8. You may not copy, modify, sublicense, link with, or distribute
-the Library except as expressly provided under this License.  Any
-attempt otherwise to copy, modify, sublicense, link with, or
-distribute the Library is void, and will automatically terminate your
-rights under this License.  However, parties who have received copies,
-or rights, from you under this License will not have their licenses
-terminated so long as such parties remain in full compliance.
-
-  9. You are not required to accept this License, since you have not
-signed it.  However, nothing else grants you permission to modify or
-distribute the Library or its derivative works.  These actions are
-prohibited by law if you do not accept this License.  Therefore, by
-modifying or distributing the Library (or any work based on the
-Library), you indicate your acceptance of this License to do so, and
-all its terms and conditions for copying, distributing or modifying
-the Library or works based on it.
-
-  10. Each time you redistribute the Library (or any work based on the
-Library), the recipient automatically receives a license from the
-original licensor to copy, distribute, link with or modify the Library
-subject to these terms and conditions.  You may not impose any further
-restrictions on the recipients' exercise of the rights granted herein.
-You are not responsible for enforcing compliance by third parties with
-this License.
-
-  11. If, as a consequence of a court judgment or allegation of patent
-infringement or for any other reason (not limited to patent issues),
-conditions are imposed on you (whether by court order, agreement or
-otherwise) that contradict the conditions of this License, they do not
-excuse you from the conditions of this License.  If you cannot
-distribute so as to satisfy simultaneously your obligations under this
-License and any other pertinent obligations, then as a consequence you
-may not distribute the Library at all.  For example, if a patent
-license would not permit royalty-free redistribution of the Library by
-all those who receive copies directly or indirectly through you, then
-the only way you could satisfy both it and this License would be to
-refrain entirely from distribution of the Library.
-
-If any portion of this section is held invalid or unenforceable under any
-particular circumstance, the balance of the section is intended to apply,
-and the section as a whole is intended to apply in other circumstances.
-
-It is not the purpose of this section to induce you to infringe any
-patents or other property right claims or to contest validity of any
-such claims; this section has the sole purpose of protecting the
-integrity of the free software distribution system which is
-implemented by public license practices.  Many people have made
-generous contributions to the wide range of software distributed
-through that system in reliance on consistent application of that
-system; it is up to the author/donor to decide if he or she is willing
-to distribute software through any other system and a licensee cannot
-impose that choice.
-
-This section is intended to make thoroughly clear what is believed to
-be a consequence of the rest of this License.
-
-  12. If the distribution and/or use of the Library is restricted in
-certain countries either by patents or by copyrighted interfaces, the
-original copyright holder who places the Library under this License may add
-an explicit geographical distribution limitation excluding those countries,
-so that distribution is permitted only in or among countries not thus
-excluded.  In such case, this License incorporates the limitation as if
-written in the body of this License.
-
-  13. The Free Software Foundation may publish revised and/or new
-versions of the Lesser General Public License from time to time.
-Such new versions will be similar in spirit to the present version,
-but may differ in detail to address new problems or concerns.
-
-Each version is given a distinguishing version number.  If the Library
-specifies a version number of this License which applies to it and
-"any later version", you have the option of following the terms and
-conditions either of that version or of any later version published by
-the Free Software Foundation.  If the Library does not specify a
-license version number, you may choose any version ever published by
-the Free Software Foundation.
-
-  14. If you wish to incorporate parts of the Library into other free
-programs whose distribution conditions are incompatible with these,
-write to the author to ask for permission.  For software which is
-copyrighted by the Free Software Foundation, write to the Free
-Software Foundation; we sometimes make exceptions for this.  Our
-decision will be guided by the two goals of preserving the free status
-of all derivatives of our free software and of promoting the sharing
-and reuse of software generally.
-
-			    NO WARRANTY
-
-  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
-WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
-EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
-OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
-KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
-IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
-PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
-LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
-THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
-
-  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
-WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
-AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
-FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
-CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
-LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
-RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
-FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
-SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
-DAMAGES.
-
-		     END OF TERMS AND CONDITIONS
-
-           How to Apply These Terms to Your New Libraries
-
-  If you develop a new library, and you want it to be of the greatest
-possible use to the public, we recommend making it free software that
-everyone can redistribute and change.  You can do so by permitting
-redistribution under these terms (or, alternatively, under the terms of the
-ordinary General Public License).
-
-  To apply these terms, attach the following notices to the library.  It is
-safest to attach them to the start of each source file to most effectively
-convey the exclusion of warranty; and each file should have at least the
-"copyright" line and a pointer to where the full notice is found.
-
-    <one line to give the library's name and a brief idea of what it does.>
-    Copyright (C) <year>  <name of author>
-
-    This library is free software; you can redistribute it and/or
-    modify it under the terms of the GNU Lesser General Public
-    License as published by the Free Software Foundation; either
-    version 2.1 of the License, or (at your option) any later version.
-
-    This library is distributed in the hope that it will be useful,
-    but WITHOUT ANY WARRANTY; without even the implied warranty of
-    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-    Lesser General Public License for more details.
-
-    You should have received a copy of the GNU Lesser General Public
-    License along with this library; if not, write to the Free Software
-    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-
-Also add information on how to contact you by electronic and paper mail.
-
-You should also get your employer (if you work as a programmer) or your
-school, if any, to sign a "copyright disclaimer" for the library, if
-necessary.  Here is a sample; alter the names:
-
-  Yoyodyne, Inc., hereby disclaims all copyright interest in the
-  library `Frob' (a library for tweaking knobs) written by James Random Hacker.
-
-  <signature of Ty Coon>, 1 April 1990
-  Ty Coon, President of Vice
-
-That's all there is to it!
-
-
diff --git a/phonelibs/qpoases/README.txt b/phonelibs/qpoases/README.txt
deleted file mode 100644
index 53fc2ab4f..000000000
--- a/phonelibs/qpoases/README.txt
+++ /dev/null
@@ -1,92 +0,0 @@
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
-##
-##	qpOASES is free software; you can redistribute it and/or
-##	modify it under the terms of the GNU Lesser General Public
-##	License as published by the Free Software Foundation; either
-##	version 2.1 of the License, or (at your option) any later version.
-##
-##	qpOASES is distributed in the hope that it will be useful,
-##	but WITHOUT ANY WARRANTY; without even the implied warranty of
-##	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-##	Lesser General Public License for more details.
-##
-##	You should have received a copy of the GNU Lesser General Public
-##	License along with qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-INTRODUCTION
-=============
-
-qpOASES is an open-source C++ implementation of the recently proposed 
-online active set strategy (see [1], [2]), which was inspired by important 
-observations from the field of parametric quadratic programming. It has 
-several theoretical features that make it particularly suited for model 
-predictive control (MPC) applications.
-
-The software package qpOASES implements these ideas and has already been 
-successfully used for closed-loop control of a real-world Diesel engine [3].
-
-
-References:
-
-[1] H.J. Ferreau. An Online Active Set Strategy for Fast Solution of 
-Parametric Quadratic Programs with Applications to Predictive Engine Control. 
-Diplom thesis, University of Heidelberg, 2006.
-
-[2] H.J. Ferreau, H.G. Bock, M. Diehl. An online active set strategy to 
-overcome the limitations of explicit MPC. International Journal of Robust 
-and Nonlinear Control, 18 (8), pp. 816-830, 2008.
-
-[3] H.J. Ferreau, P. Ortner, P. Langthaler, L. del Re, M. Diehl. Predictive 
-Control of a Real-World Diesel Engine using an Extended Online Active Set 
-Strategy. Annual Reviews in Control, 31 (2), pp. 293-301, 2007.
-
-
-
-GETTING STARTED
-================
-
-1. For installation, usage and additional information on this software package 
-   see the qpOASES User's Manual located at ./DOC/manual.pdf!
-
-
-2. The file ./LICENSE.txt contains a copy of the GNU Lesser General Public 
-   License. Please read it carefully before using qpOASES!
-
-
-3. The whole software package can be downloaded from 
-
-        http://homes.esat.kuleuven.be/~optec/software/qpOASES/ 
-
-   On this webpage you will also find a list of frequently asked questions.
-
-
-
-CONTACT THE AUTHORS
-====================
-
-If you have got questions, remarks or comments on qpOASES 
-please contact the main author:
-
-        Hans Joachim Ferreau
-        Katholieke Universiteit Leuven
-        Department of Electrical Engineering (ESAT)
-        Kasteelpark Arenberg 10, bus 2446
-        B-3001 Leuven-Heverlee, Belgium
-
-        Phone: +32 16 32 03 63
-        E-mail: joachim.ferreau@esat.kuleuven.be
-                qpOASES@esat.kuleuven.be
-
-Also bug reports and source code extensions are most welcome!
-
-
-
-##
-##	end of file
-##
diff --git a/phonelibs/qpoases/SConscript b/phonelibs/qpoases/SConscript
deleted file mode 100644
index 932dbc700..000000000
--- a/phonelibs/qpoases/SConscript
+++ /dev/null
@@ -1,16 +0,0 @@
-Import('env', 'interface_dir')
-
-qp_files = [
-    Glob("SRC/*.cpp"),
-    Glob("SRC/EXTRAS/*.cpp"),
-]
-
-cpp_path = [
-    ".",
-    "INCLUDE",
-    "INCLUDE/EXTRAS",
-    "SRC/",
-    interface_dir,
-]
-
-env.Library('qpoases', qp_files,  CPPPATH=cpp_path)
diff --git a/phonelibs/qpoases/SRC/Bounds.cpp b/phonelibs/qpoases/SRC/Bounds.cpp
deleted file mode 100644
index cf8ed6989..000000000
--- a/phonelibs/qpoases/SRC/Bounds.cpp
+++ /dev/null
@@ -1,252 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Bounds.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the Bounds class designed to manage working sets of
- *	bounds within a QProblem.
- */
-
-
-#include <Bounds.hpp>
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	B o u n d s
- */
-Bounds::Bounds( ) :	SubjectTo( ),
-					nV( 0 ),
-					nFV( 0 ),
-					nBV( 0 ),
-					nUV( 0 )
-{
-}
-
-
-/*
- *	B o u n d s
- */
-Bounds::Bounds( const Bounds& rhs ) :	SubjectTo( rhs ),
-										nV( rhs.nV ),
-										nFV( rhs.nFV ),
-										nBV( rhs.nBV ),
-										nUV( rhs.nUV )
-{
-	free  = rhs.free;
-	fixed = rhs.fixed;
-}
-
-
-/*
- *	~ B o u n d s
- */
-Bounds::~Bounds( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Bounds& Bounds::operator=( const Bounds& rhs )
-{
-	if ( this != &rhs )
-	{
-		SubjectTo::operator=( rhs );
-
-		nV  = rhs.nV;
-		nFV = rhs.nFV;
-		nBV = rhs.nBV;
-		nUV = rhs.nUV;
-
-		free  = rhs.free;
-		fixed = rhs.fixed;
-	}
-
-	return *this;
-}
-
-
-/*
- *	i n i t
- */
-returnValue Bounds::init( int n )
-{
-	nV = n;
-	nFV = 0;
-	nBV = 0;
-	nUV = 0;
-
-	free.init( );
-	fixed.init( );
-
-	return SubjectTo::init( n );
-}
-
-
-/*
- *	s e t u p B o u n d
- */
-returnValue Bounds::setupBound(	int _number, SubjectToStatus _status
-								)
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	
-	/* Add bound index to respective index list. */
-	switch ( _status )
-	{
-		case ST_INACTIVE:
-			if ( this->addIndex( this->getFree( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		case ST_LOWER:
-			if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		case ST_UPPER:
-			if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-			break;
-
-		default:
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A l l F r e e
- */
-returnValue Bounds::setupAllFree( )
-{
-	int i;
-
-	/* 1) Place unbounded variables at the beginning of the index list of free variables. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( getType( i ) == ST_UNBOUNDED )
-		{
-			if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	/* 2) Add remaining (i.e. bounded but possibly free) variables to the index list of free variables. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( getType( i ) == ST_BOUNDED )
-		{
-			if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	/* 3) Place implicitly fixed variables at the end of the index list of free variables. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( getType( i ) == ST_EQUALITY )
-		{
-			if ( setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_BOUND_FAILED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e F i x e d T o F r e e
- */
-returnValue Bounds::moveFixedToFree( int _number )
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of fixed variables to that of free ones. */
-	if ( this->removeIndex( this->getFixed( ),_number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getFree( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e F r e e T o F i x e d
- */
-returnValue Bounds::moveFreeToFixed(	int _number, SubjectToStatus _status
-										)
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNV( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of free variables to that of fixed ones. */
-	if ( this->removeIndex( this->getFree( ),_number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getFixed( ),_number,_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p F r e e
- */
-returnValue Bounds::swapFree(	int number1, int number2
-								)
-{
-	/* consistency check */
-	if ( ( number1 < 0 ) || ( number1 >= getNV( ) ) || ( number2 < 0 ) || ( number2 >= getNV( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Swap index within indexlist of free variables. */
-	return this->swapIndex( this->getFree( ),number1,number2 );
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Bounds.ipp b/phonelibs/qpoases/SRC/Bounds.ipp
deleted file mode 100644
index d2ab1ba2d..000000000
--- a/phonelibs/qpoases/SRC/Bounds.ipp
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Bounds.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the Bounds class designed 
- *	to manage working sets of bounds within a QProblem.
- */
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t N V
- */
-inline int Bounds::getNV( ) const
-{
- 	return nV;
-}
-
-
-/*
- *	g e t N F X
- */
-inline int Bounds::getNFV( ) const
-{
- 	return nFV;
-}
-
-
-/*
- *	g e t N B V
- */
-inline int Bounds::getNBV( ) const
-{
- 	return nBV;
-}
- 
-
-/*
- *	g e t N U V
- */
-inline int Bounds::getNUV( ) const
-{
-	return nUV;
-}
-
-
-
-/*
- *	s e t N F X
- */
-inline returnValue Bounds::setNFV( int n )
-{
- 	nFV = n;
-	return SUCCESSFUL_RETURN;	
-}
- 
- 
-/*
- *	s e t N B V
- */
-inline returnValue Bounds::setNBV( int n )
-{
- 	nBV = n;
-	return SUCCESSFUL_RETURN;
-}
- 
-
-/*
- *	s e t N U V
- */
-inline returnValue Bounds::setNUV( int n )
-{
-	nUV = n;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N F R
- */
-inline int Bounds::getNFR( )
-{
- 	return free.getLength( );
-}
-
-
-/*
- *	g e t N F X
- */
-inline int Bounds::getNFX( )
-{
- 	return fixed.getLength( );
-}
-
-
-/*
- *	g e t F r e e
- */
-inline Indexlist* Bounds::getFree( )
-{
-	return &free;
-}
-
-
-/*
- *	g e t F i x e d
- */
-inline Indexlist* Bounds::getFixed( )
-{
-	return &fixed;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Constraints.cpp b/phonelibs/qpoases/SRC/Constraints.cpp
deleted file mode 100644
index b2ad5bd11..000000000
--- a/phonelibs/qpoases/SRC/Constraints.cpp
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Constraints.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the Constraints class designed to manage working sets of
- *	constraints within a QProblem.
- */
-
-
-#include <Constraints.hpp>
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	C o n s t r a i n t s
- */
-Constraints::Constraints( ) :	SubjectTo( ),
-								nC( 0 ),
-								nEC( 0 ),
-								nIC( 0 ),
-								nUC( 0 )
-{
-}
-
-
-/*
- *	C o n s t r a i n t s
- */
-Constraints::Constraints( const Constraints& rhs ) :	SubjectTo( rhs ),
-														nC( rhs.nC ),
-														nEC( rhs.nEC ),
-														nIC( rhs.nIC ),
-														nUC( rhs.nUC )
-{
-	active =   rhs.active;
-	inactive = rhs.inactive;
-}
-
-
-/*
- *	~ C o n s t r a i n t s
- */
-Constraints::~Constraints( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Constraints& Constraints::operator=( const Constraints& rhs )
-{
-	if ( this != &rhs )
-	{
-		SubjectTo::operator=( rhs );
-
-		nC  = rhs.nC;
-		nEC = rhs.nEC;
-		nIC = rhs.nIC;
-		nUC = rhs.nUC;
-
-		active =   rhs.active;
-		inactive = rhs.inactive;
-	}
-
-	return *this;
-}
-
-
-/*
- *	i n i t
- */
-returnValue Constraints::init( int n )
-{
-	nC = n;
-	nEC = 0;
-	nIC = 0;
-	nUC = 0;
-
-	active.init( );
-	inactive.init( );
-
-	return SubjectTo::init( n );
-}
-
-
-/*
- *	s e t u p C o n s t r a i n t
- */
-returnValue Constraints::setupConstraint(	int _number, SubjectToStatus _status
-											)
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	
-	/* Add constraint index to respective index list. */
-	switch ( _status )
-	{
-		case ST_INACTIVE:
-			if ( this->addIndex( this->getInactive( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		case ST_LOWER:
-			if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		case ST_UPPER:
-			if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-			break;
-
-		default:
-			return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A l l I n a c t i v e
- */
-returnValue Constraints::setupAllInactive( )
-{
-	int i;
-
-
-	/* 1) Place unbounded constraints at the beginning of the index list of inactive constraints. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( getType( i ) == ST_UNBOUNDED )
-		{
-			if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 2) Add remaining (i.e. "real" inequality) constraints to the index list of inactive constraints. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( getType( i ) == ST_BOUNDED )
-		{
-			if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 3) Place implicit equality constraints at the end of the index list of inactive constraints. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( getType( i ) == ST_EQUALITY )
-		{
-			if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	/* 4) Moreover, add all constraints of unknown type. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( getType( i ) == ST_UNKNOWN )
-		{
-			if ( setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_CONSTRAINT_FAILED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e A c t i v e T o I n a c t i v e
- */
-returnValue Constraints::moveActiveToInactive( int _number )
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of active constraints to that of inactive ones. */
-	if ( this->removeIndex( this->getActive( ),_number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getInactive( ),_number,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	m o v e I n a c t i v e T o A c t i v e
- */
-returnValue Constraints::moveInactiveToActive(	int _number, SubjectToStatus _status
-												)
-{
-	/* consistency check */
-	if ( ( _number < 0 ) || ( _number >= getNC( ) ) )
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-
-	/* Move index from indexlist of inactive constraints to that of active ones. */
-	if ( this->removeIndex( this->getInactive( ),_number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	if ( this->addIndex( this->getActive( ),_number,_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_MOVING_BOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Constraints.ipp b/phonelibs/qpoases/SRC/Constraints.ipp
deleted file mode 100644
index 1b874ef3c..000000000
--- a/phonelibs/qpoases/SRC/Constraints.ipp
+++ /dev/null
@@ -1,144 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Constraints.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Declaration of inlined member functions of the Constraints class designed 
- *	to manage working sets of constraints within a QProblem.
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t N C
- */
-inline int Constraints::getNC( ) const
-{
- 	return nC;
-}
- 
-
-/*
- *	g e t N E C
- */
-inline int Constraints::getNEC( ) const
-{
- 	return nEC;
-}
- 
-
-/*
- *	g e t N I C
- */
-inline int Constraints::getNIC( ) const
-{
- 	return nIC;
-}
- 
-
-/*
- *	g e t N U C
- */
-inline int Constraints::getNUC( ) const
-{
- 	return nUC;
-}
-
-
-/*
- *	s e t N E C
- */
-inline returnValue Constraints::setNEC( int n )
-{
- 	nEC = n;
-	return SUCCESSFUL_RETURN;
-}
- 
-
-/*
- *	s e t N I C
- */
-inline returnValue Constraints::setNIC( int n )
-{
- 	nIC = n;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t N U C
- */
-inline returnValue Constraints::setNUC( int n )
-{
- 	nUC = n;
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N A C
- */
-inline int Constraints::getNAC( )
-{
- 	return active.getLength( );
-}
-
-
-/*
- *	g e t N I A C
- */
-inline int Constraints::getNIAC( )
-{
- 	return inactive.getLength( );
-}
-
-
-/*
- *	g e t A c t i v e
- */
-inline Indexlist* Constraints::getActive( )
-{
-	return &active;
-}
-
-
-/*
- *	g e t I n a c t i v e
- */
-inline Indexlist* Constraints::getInactive( )
-{
-	return &inactive;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/CyclingManager.cpp b/phonelibs/qpoases/SRC/CyclingManager.cpp
deleted file mode 100644
index 27c2996ef..000000000
--- a/phonelibs/qpoases/SRC/CyclingManager.cpp
+++ /dev/null
@@ -1,188 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/CyclingManager.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the CyclingManager class designed to detect
- *	and handle possible cycling during QP iterations.
- *
- */
-
-
-#include <CyclingManager.hpp>
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	C y c l i n g M a n a g e r
- */
-CyclingManager::CyclingManager( ) :	nV( 0 ),
-									nC( 0 )
-{
-	cyclingDetected = BT_FALSE;
-}
-
-
-/*
- *	C y c l i n g M a n a g e r
- */
-CyclingManager::CyclingManager( const CyclingManager& rhs ) :	nV( rhs.nV ),
-																nC( rhs.nC ),
-																cyclingDetected( rhs.cyclingDetected )
-{
-	int i;
-
-	for( i=0; i<nV+nC; ++i )
-		status[i] = rhs.status[i];
-}
-
-
-/*
- *	~ C y c l i n g M a n a g e r
- */
-CyclingManager::~CyclingManager( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-CyclingManager& CyclingManager::operator=( const CyclingManager& rhs )
-{
-	int i;
-
-	if ( this != &rhs )
-	{
-		nV = rhs.nV;
-		nC = rhs.nC;
-
-		for( i=0; i<nV+nC; ++i )
-			status[i] = rhs.status[i];
-
-		cyclingDetected = rhs.cyclingDetected;
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	i n i t
- */
-returnValue CyclingManager::init( int _nV, int _nC )
-{
-	nV = _nV;
-	nC = _nC;
-
-	cyclingDetected = BT_FALSE;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t C y c l i n g S t a t u s
- */
-returnValue CyclingManager::setCyclingStatus(	int number,
-												BooleanType isBound, CyclingStatus _status
-												)
-{
-	if ( isBound == BT_TRUE )
-	{
-		/* Set cycling status of a bound. */
-		if ( ( number >= 0 ) && ( number < nV ) )
-		{
-			status[number] = _status;
-			return SUCCESSFUL_RETURN;
-		}
-		else
-			return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-	else
-	{
-		/* Set cycling status of a constraint. */
-		if ( ( number >= 0 ) && ( number < nC ) )
-		{
-			status[nV+number] = _status;
-			return SUCCESSFUL_RETURN;
-		}
-		else
-			return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	g e t C y c l i n g S t a t u s
- */
-CyclingStatus CyclingManager::getCyclingStatus( int number, BooleanType isBound ) const
-{
-	if ( isBound == BT_TRUE )
-	{
-		/* Return cycling status of a bound. */
-		if ( ( number >= 0 ) && ( number < nV ) )
-			return status[number];
-	}
-	else
-	{
-		/* Return cycling status of a constraint. */
-		if ( ( number >= 0 ) && ( number < nC ) )
-			return status[nV+number];
-	}
-
-	return CYC_NOT_INVOLVED;
-}
-
-
-/*
- *	c l e a r C y c l i n g D a t a
- */
-returnValue CyclingManager::clearCyclingData( )
-{
-	int i;
-
-	/* Reset all status values ... */
-	for( i=0; i<nV+nC; ++i )
-		status[i] = CYC_NOT_INVOLVED;
-
-	/* ... and the main cycling flag. */
-	cyclingDetected = BT_FALSE;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/CyclingManager.ipp b/phonelibs/qpoases/SRC/CyclingManager.ipp
deleted file mode 100644
index 386409a53..000000000
--- a/phonelibs/qpoases/SRC/CyclingManager.ipp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/CyclingManager.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the CyclingManager class 
- *	designed to detect and handle possible cycling during QP iterations.
- *	
- */
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	i s C y c l i n g D e t e c t e d
- */
-inline BooleanType CyclingManager::isCyclingDetected( ) const
-{
-	return cyclingDetected;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp b/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp
deleted file mode 100644
index c744adc0d..000000000
--- a/phonelibs/qpoases/SRC/EXTRAS/SolutionAnalysis.cpp
+++ /dev/null
@@ -1,434 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/EXTRAS/SolutionAnalysis.cpp
- *	\author Milan Vukov, Boris Houska, Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2012
- *
- *	Solution analysis class, based on a class in the standard version of the qpOASES
- */
-
-#include <EXTRAS/SolutionAnalysis.hpp>
-
-/*
- *	S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::SolutionAnalysis( )
-{
-	
-}
-
-/*
- *	S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::SolutionAnalysis( const SolutionAnalysis& rhs )
-{
-	
-}
-
-/*
- *	~ S o l u t i o n A n a l y s i s
- */
-SolutionAnalysis::~SolutionAnalysis( )
-{
-	
-}
-
-/*
- *	o p e r a t o r =
- */
-SolutionAnalysis& SolutionAnalysis::operator=( const SolutionAnalysis& rhs )
-{
-	if ( this != &rhs )
-	{
-		
-	}
-	
-	return *this;
-}
-
-/*
- * g e t H e s s i a n I n v e r s e
- */
-returnValue SolutionAnalysis::getHessianInverse( QProblem* qp, real_t* hessianInverse )
-{
-	returnValue returnvalue; /* the return value */
-	BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
-	BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
-	
-	register int run1, run2, run3;
-	
-	register int nFR, nFX;
-	
-	/* Ask for the number of free and fixed variables, assumes that active set
-	 * is constant for the covariance evaluation */
-	nFR = qp->getNFR( );
-	nFX = qp->getNFX( );
-	
-	/* Ask for the corresponding index arrays: */
-	if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	/* Initialization: */
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_g_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_lb_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_ub_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NCMAX; run1++ )
-		delta_lbA_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NCMAX; run1++ )
-		delta_ubA_cov[ run1 ] = 0.0;
-	
-	/* The following loop solves the following:
-	 *
-	 * KKT * x =
-	 *   [delta_g_cov', delta_lbA_cov', delta_ubA_cov', delta_lb_cov', delta_ub_cov]'
-	 *
-	 * for the first NVMAX (negative) elementary vectors in order to get
-	 * transposed inverse of the Hessian. Assuming that the Hessian is
-	 * symmetric, the function will return transposed inverse, instead of the
-	 * true inverse.
-	 *
-	 * Note, that we use negative elementary vectors due because internal
-	 * implementation of the function hotstart_determineStepDirection requires
-	 * so.
-	 *
-	 * */
-	
-	for( run3 = 0; run3 < NVMAX; run3++ )
-	{
-		/* Line wise loading of the corresponding (negative) elementary vector: */
-		delta_g_cov[ run3 ] = -1.0;
-		
-		/* Evaluation of the step: */
-		returnvalue = qp->hotstart_determineStepDirection(
-			FR_idx, FX_idx, AC_idx,
-			delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
-			Delta_bC_isZero, Delta_bB_isZero,
-			delta_xFX, delta_xFR, delta_yAC, delta_yFX
-			);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			return returnvalue;
-		}
-		
-		/* Line wise storage of the QP reaction: */
-		for( run1 = 0; run1 < nFR; run1++ )
-		{
-			run2 = FR_idx[ run1 ];
-			
-			hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
-		} 
-		
-		for( run1 = 0; run1 < nFX; run1++ )
-		{ 
-			run2 = FX_idx[ run1 ];
-			
-			hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
-		}
-		
-		/* Prepare for the next iteration */
-		delta_g_cov[ run3 ] = 0.0;
-	}
-	
-	// TODO: Perform the transpose of the inverse of the Hessian matrix
-	
-	return SUCCESSFUL_RETURN; 
-}
-
-/*
- * g e t H e s s i a n I n v e r s e
- */
-returnValue SolutionAnalysis::getHessianInverse( QProblemB* qp, real_t* hessianInverse )
-{
-	returnValue returnvalue; /* the return value */
-	BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
-	
-	register int run1, run2, run3;
-	
-	register int nFR, nFX;
-	
-	/* Ask for the number of free and fixed variables, assumes that active set
-	 * is constant for the covariance evaluation */
-	nFR = qp->getNFR( );
-	nFX = qp->getNFX( );
-	
-	/* Ask for the corresponding index arrays: */
-	if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	/* Initialization: */
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_g_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_lb_cov[ run1 ] = 0.0;
-	
-	for( run1 = 0; run1 < NVMAX; run1++ )
-		delta_ub_cov[ run1 ] = 0.0;
-	
-	/* The following loop solves the following:
-	 *
-	 * KKT * x =
-	 *   [delta_g_cov', delta_lb_cov', delta_ub_cov']'
-	 *
-	 * for the first NVMAX (negative) elementary vectors in order to get
-	 * transposed inverse of the Hessian. Assuming that the Hessian is
-	 * symmetric, the function will return transposed inverse, instead of the
-	 * true inverse.
-	 *
-	 * Note, that we use negative elementary vectors due because internal
-	 * implementation of the function hotstart_determineStepDirection requires
-	 * so.
-	 *
-	 * */
-	
-	for( run3 = 0; run3 < NVMAX; run3++ )
-	{
-		/* Line wise loading of the corresponding (negative) elementary vector: */
-		delta_g_cov[ run3 ] = -1.0;
-		
-		/* Evaluation of the step: */
-		returnvalue = qp->hotstart_determineStepDirection(
-			FR_idx, FX_idx,
-			delta_g_cov, delta_lb_cov, delta_ub_cov,
-			Delta_bB_isZero,
-			delta_xFX, delta_xFR, delta_yFX
-			);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			return returnvalue;
-		}
-				
-		/* Line wise storage of the QP reaction: */
-		for( run1 = 0; run1 < nFR; run1++ )
-		{
-			run2 = FR_idx[ run1 ];
-			
-			hessianInverse[run3 * NVMAX + run2] = delta_xFR[ run1 ];
-		} 
-		
-		for( run1 = 0; run1 < nFX; run1++ )
-		{ 
-			run2 = FX_idx[ run1 ];
-			
-			hessianInverse[run3 * NVMAX + run2] = delta_xFX[ run1 ];
-		}
-		
-		/* Prepare for the next iteration */
-		delta_g_cov[ run3 ] = 0.0;
-	}
-	
-	// TODO: Perform the transpose of the inverse of the Hessian matrix
-	
-	return SUCCESSFUL_RETURN; 
-}
-
-/*
- * g e t V a r i a n c e C o v a r i a n c e
- */
-
-#if QPOASES_USE_OLD_VERSION
-
-returnValue SolutionAnalysis::getVarianceCovariance( QProblem* qp, real_t* g_b_bA_VAR, real_t* Primal_Dual_VAR )
-{
-	int run1, run2, run3; /* simple run variables (for loops). */
-	
-	returnValue returnvalue; /* the return value */
-	BooleanType Delta_bC_isZero = BT_FALSE; /* (just use FALSE here) */
-	BooleanType Delta_bB_isZero = BT_FALSE; /* (just use FALSE here) */
-	
-	/* ASK FOR THE NUMBER OF FREE AND FIXED VARIABLES:
-	 * (ASSUMES THAT ACTIVE SET IS CONSTANT FOR THE
-	 *  VARIANCE-COVARIANCE EVALUATION)
-	 * ----------------------------------------------- */
-	int nFR, nFX, nAC;
-	
-	nFR = qp->getNFR( );
-	nFX = qp->getNFX( );
-	nAC = qp->getNAC( );
-	
-	if ( qp->bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	if ( qp->bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	if ( qp->constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_HOTSTART_FAILED );
-	
-	/* SOME INITIALIZATIONS:
-	 * --------------------- */
-	for( run1 = 0; run1 < KKT_DIM * KKT_DIM; run1++ )
-	{
-		K [run1] = 0.0;
-		Primal_Dual_VAR[run1] = 0.0;
-	}
-	
-	/* ================================================================= */
-	
-	/* FIRST MATRIX MULTIPLICATION (OBTAINS THE INTERMEDIATE RESULT
-	 *  K := [ ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * g_b_bA_VAR ]^T )
-	 * THE EVALUATION OF THE INVERSE OF THE KKT-MATRIX OF THE QP
-	 * WITH RESPECT TO THE CURRENT ACTIVE SET
-	 * USES THE EXISTING CHOLESKY AND TQ-DECOMPOSITIONS. FOR DETAILS
-	 * cf. THE (protected) FUNCTION determineStepDirection. */
-	
-	for( run3 = 0; run3 < KKT_DIM; run3++ )
-	{
-		
-		for( run1 = 0; run1 < NVMAX; run1++ )
-		{
-			delta_g_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+run1];
-			delta_lb_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /*  LINE-WISE LOADING OF THE INPUT */
-			delta_ub_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+NVMAX+run1]; /*  VARIANCE-COVARIANCE            */
-		}
-		for( run1 = 0; run1 < NCMAX; run1++ )
-		{
-			delta_lbA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
-			delta_ubA_cov [run1] = g_b_bA_VAR[run3*KKT_DIM+2*NVMAX+run1];
-		}
-		
-		/* EVALUATION OF THE STEP:
-		 * ------------------------------------------------------------------------------ */
-		
-		returnvalue = qp->hotstart_determineStepDirection(
-			FR_idx, FX_idx, AC_idx,
-			delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
-			Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
-			delta_yAC,delta_yFX );
-		
-		/* ------------------------------------------------------------------------------ */
-		
-		/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
-		 * ------------------------------------------------------ */
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			return returnvalue;
-		}
-		
-		/*  LINE WISE                  */
-		/*  STORAGE OF THE QP-REACTION */
-		/*  (uses the index list)      */
-		
-		for( run1=0; run1<nFR; run1++ )
-		{
-			run2 = FR_idx[run1];
-			K[run3*KKT_DIM+run2] = delta_xFR[run1];
-		} 
-		for( run1=0; run1<nFX; run1++ )
-		{ 
-			run2 = FX_idx[run1]; 
-			K[run3*KKT_DIM+run2] = delta_xFX[run1];
-			K[run3*KKT_DIM+NVMAX+run2] = delta_yFX[run1];
-		}
-		for( run1=0; run1<nAC; run1++ )
-		{
-			run2 = AC_idx[run1];
-			K[run3*KKT_DIM+2*NVMAX+run2] = delta_yAC[run1];
-		}
-	}
-	
-	/* ================================================================= */
-	
-	/* SECOND MATRIX MULTIPLICATION (OBTAINS THE FINAL RESULT
-	 * Primal_Dual_VAR := ("ACTIVE" KKT-MATRIX OF THE QP)^(-1) * K )
-	 * THE APPLICATION OF THE KKT-INVERSE IS AGAIN REALIZED
-	 * BY USING THE PROTECTED FUNCTION
-	 * determineStepDirection */
-	
-	for( run3 = 0; run3 < KKT_DIM; run3++ )
-	{
-		
-		for( run1 = 0; run1 < NVMAX; run1++ )
-		{
-			delta_g_cov [run1] = K[run3+ run1*KKT_DIM];
-			delta_lb_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /*  ROW WISE LOADING OF THE */
-			delta_ub_cov [run1] = K[run3+(NVMAX+run1)*KKT_DIM]; /*  INTERMEDIATE RESULT K   */
-		}
-		for( run1 = 0; run1 < NCMAX; run1++ )
-		{
-			delta_lbA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
-			delta_ubA_cov [run1] = K[run3+(2*NVMAX+run1)*KKT_DIM];
-		}
-		
-		/* EVALUATION OF THE STEP:
-		 * ------------------------------------------------------------------------------ */
-		
-		returnvalue = qp->hotstart_determineStepDirection(
-			FR_idx, FX_idx, AC_idx,
-			delta_g_cov, delta_lbA_cov, delta_ubA_cov, delta_lb_cov, delta_ub_cov,
-			Delta_bC_isZero, Delta_bB_isZero, delta_xFX,delta_xFR,
-			delta_yAC,delta_yFX );
-		
-		/* ------------------------------------------------------------------------------ */
-		
-		/* STOP THE ALGORITHM IN THE CASE OF NO SUCCESFUL RETURN:
-		 * ------------------------------------------------------ */
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			return returnvalue;
-		}
-		
-		/*  ROW-WISE STORAGE */
-		/*  OF THE RESULT.   */
-		
-		for( run1=0; run1<nFR; run1++ )
-		{
-			run2 = FR_idx[run1];
-			Primal_Dual_VAR[run3+run2*KKT_DIM] = delta_xFR[run1];
-		}
-		for( run1=0; run1<nFX; run1++ )
-		{ 
-			run2 = FX_idx[run1]; 
-			Primal_Dual_VAR[run3+run2*KKT_DIM ] = delta_xFX[run1];
-			Primal_Dual_VAR[run3+(NVMAX+run2)*KKT_DIM] = delta_yFX[run1];
-		}
-		for( run1=0; run1<nAC; run1++ )
-		{
-			run2 = AC_idx[run1];
-			Primal_Dual_VAR[run3+(2*NVMAX+run2)*KKT_DIM] = delta_yAC[run1];
-		}
-	}
-	
-	return SUCCESSFUL_RETURN;
-}
-
-#endif
diff --git a/phonelibs/qpoases/SRC/Indexlist.cpp b/phonelibs/qpoases/SRC/Indexlist.cpp
deleted file mode 100644
index b512569a8..000000000
--- a/phonelibs/qpoases/SRC/Indexlist.cpp
+++ /dev/null
@@ -1,342 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Indexlist.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the Indexlist class designed to manage index lists of
- *	constraints and bounds within a QProblem_SubjectTo.
- */
-
-
-#include <Indexlist.hpp>
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	I n d e x l i s t
- */
-Indexlist::Indexlist( ) :	length( 0 ),
-							first( -1 ),
-							last( -1 ),
-							lastusedindex( -1 ),
-							physicallength( INDEXLISTFACTOR*(NVMAX+NCMAX) )
-{
-	int i;
-
-	for( i=0; i<physicallength; ++i )
-	{
-		number[i] = -1;
-		next[i] = -1;
-		previous[i] = -1;
-	}
-}
-
-
-/*
- *	I n d e x l i s t
- */
-Indexlist::Indexlist( const Indexlist& rhs ) :	length( rhs.length ),
-												first( rhs.first ),
-												last( rhs.last ),
-												lastusedindex( rhs.lastusedindex ),
-												physicallength( rhs.physicallength )
-{
-	int i;
-
-	for( i=0; i<physicallength; ++i )
-	{
-		number[i] = rhs.number[i];
-		next[i] = rhs.next[i];
-		previous[i] = rhs.previous[i];
-	}
-}
-
-
-/*
- *	~ I n d e x l i s t
- */
-Indexlist::~Indexlist( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-Indexlist& Indexlist::operator=( const Indexlist& rhs )
-{
-	int i;
-
-	if ( this != &rhs )
-	{
-		length = rhs.length;
-		first = rhs.first;
-		last = rhs.last;
-		lastusedindex = rhs.lastusedindex;
-		physicallength = rhs.physicallength;
-
-		for( i=0; i<physicallength; ++i )
-		{
-			number[i] = rhs.number[i];
-			next[i] = rhs.next[i];
-			previous[i] = rhs.previous[i];
-		}
-	}
-
-	return *this;
-}
-
-
-/*
- *	i n i t
- */
-returnValue Indexlist::init( )
-{
-	int i;
-
-	length = 0;
-	first = -1;
-	last = -1;
-	lastusedindex = -1;
-	physicallength = INDEXLISTFACTOR*(NVMAX+NCMAX);
-
-	for( i=0; i<physicallength; ++i )
-	{
-		number[i] = -1;
-		next[i] = -1;
-		previous[i] = -1;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N u m b e r A r r a y
- */
-returnValue Indexlist::getNumberArray( int* const numberarray ) const
-{
-	int i;
-	int n = first;
-
-	/* Run trough indexlist and store numbers in numberarray. */
-	for( i=0; i<length; ++i )
-	{
-		if ( ( n >= 0 ) && ( number[n] >= 0 ) )
-			numberarray[i] = number[n];
-		else
-			return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-		n = next[n];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t I n d e x
- */
-int Indexlist::getIndex( int givennumber ) const
-{
-	int i;
-	int n = first;
-	int index = -1;	/* return -1 by default */
-
-	/* Run trough indexlist until number is found, if so return it index. */
-	for ( i=0; i<length; ++i )
-	{
-		if ( number[n] == givennumber )
-		{
-			index = i;
-			break;
-		}
-
-		n = next[n];
-	}
-
-	return index;
-}
-
-
-/*
- *	g e t P h y s i c a l I n d e x
- */
-int Indexlist::getPhysicalIndex( int givennumber ) const
-{
-	int i;
-	int n = first;
-	int index = -1;	/* return -1 by default */
-
-	/* Run trough indexlist until number is found, if so return it physicalindex. */
-	for ( i=0; i<length; ++i )
-	{
-		if ( number[n] == givennumber )
-		{
-			index = n;
-			break;
-		}
-
-		n = next[n];
-	}
-
-	return index;
-}
-
-
-/*
- *	a d d N u m b e r
- */
-returnValue Indexlist::addNumber( int addnumber )
-{
-	int i;
-
-	if ( lastusedindex+1 < physicallength )
-	{
-		/* If there is enough storage, add number to indexlist. */
-		++lastusedindex;
-		number[lastusedindex] = addnumber;
-		next[lastusedindex] = 0;
-
-		if ( length == 0 )
-		{
-			first = lastusedindex;
-			previous[lastusedindex] = 0;
-		}
-		else
-		{
-			next[last] = lastusedindex;
-			previous[lastusedindex] = last;
-		}
-
-		last = lastusedindex;
-		++length;
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		/* Rearrangement of index list necessary! */
-		if ( length == physicallength )
-			return THROWERROR( RET_INDEXLIST_EXCEEDS_MAX_LENGTH );
-		else
-		{
-			int numberArray[NVMAX+NCMAX];
-			getNumberArray( numberArray );
-
-			/* copy existing elements */
-			for ( i=0; i<length; ++i )
-			{
-				number[i] = numberArray[i];
-				next[i] = i+1;
-				previous[i] = i-1;
-			}
-
-			/* add new number at end of list */
-			number[length] = addnumber;
-			next[length] = -1;
-			previous[length] = length-1;
-
-			/* and set remaining entries to empty */
-			for ( i=length+1; i<physicallength; ++i )
-			{
-				number[i] = -1;
-				next[i] = -1;
-				previous[i] = -1;
-			}
-
-			first = 0;
-			last = length;
-			lastusedindex = length;
-			++length;
-
-			return THROWWARNING( RET_INDEXLIST_MUST_BE_REORDERD );
-		}
-	}
-}
-
-
-/*
- *	r e m o v e N u m b e r
- */
-returnValue Indexlist::removeNumber( int removenumber )
-{
-	int i = getPhysicalIndex( removenumber );
-
-	/* nothing to be done if number is not contained in index set */
-	if ( i < 0 )
-		return SUCCESSFUL_RETURN;
-
-	int p = previous[i];
-	int n = next[i];
-
-	if ( i == last )
-		last = p;
-	else
-		previous[n] = p;
-
-	if ( i == first )
-		first = n;
-	else
-		next[p] = n;
-
-	number[i] = -1;
-	next[i] = -1;
-	previous[i] = -1;
-	--length;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p N u m b e r s
- */
-returnValue Indexlist::swapNumbers( int number1, int number2 )
-{
-	int index1 = getPhysicalIndex( number1 );
-	int index2 = getPhysicalIndex( number2 );
-
-	/* consistency check */
-	if ( ( index1 < 0 ) || ( index2 < 0 ) )
-		return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-	int tmp = number[index1];
-	number[index1] = number[index2];
-	number[index2] = tmp;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Indexlist.ipp b/phonelibs/qpoases/SRC/Indexlist.ipp
deleted file mode 100644
index b75db81ae..000000000
--- a/phonelibs/qpoases/SRC/Indexlist.ipp
+++ /dev/null
@@ -1,85 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Indexlist.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the Indexlist class designed 
- *	to manage index lists of constraints and bounds within a QProblem_SubjectTo.
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t N u m b e r
- */
-inline int Indexlist::getNumber( int physicalindex ) const
-{
-	/* consistency check */
-	if ( ( physicalindex < 0 ) || ( physicalindex > length ) )
-		return -RET_INDEXLIST_OUTOFBOUNDS;
-
-	return number[physicalindex];
-}
-
-
-/*
- *	g e t L e n g t h
- */
-inline int Indexlist::getLength( )
-{
-	return length;
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-inline int Indexlist::getLastNumber( ) const
-{
-	return number[last];
-}
-
-
-/*
- *	g e t L a s t N u m b e r
- */
-inline BooleanType Indexlist::isMember( int _number ) const
-{
-	if ( getIndex( _number ) >= 0 )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/MessageHandling.cpp b/phonelibs/qpoases/SRC/MessageHandling.cpp
deleted file mode 100644
index 18a7654a1..000000000
--- a/phonelibs/qpoases/SRC/MessageHandling.cpp
+++ /dev/null
@@ -1,529 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/MessageHandling.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the MessageHandling class including global return values.
- *
- */
-
-
-
-#include <MessageHandling.hpp>
-#include <Utils.hpp>
-
-
-
-
-/** Defines pairs of global return values and messages. */
-MessageHandling::ReturnValueList returnValueList[] =
-{
-/* miscellaneous */
-{ SUCCESSFUL_RETURN, "Successful return", VS_VISIBLE },
-{ RET_DIV_BY_ZERO, "Division by zero", VS_VISIBLE },
-{ RET_INDEX_OUT_OF_BOUNDS, "Index out of bounds", VS_VISIBLE },
-{ RET_INVALID_ARGUMENTS, "At least one of the arguments is invalid", VS_VISIBLE },
-{ RET_ERROR_UNDEFINED, "Error number undefined", VS_VISIBLE },
-{ RET_WARNING_UNDEFINED, "Warning number undefined", VS_VISIBLE },
-{ RET_INFO_UNDEFINED, "Info number undefined", VS_VISIBLE },
-{ RET_EWI_UNDEFINED, "Error/warning/info number undefined", VS_VISIBLE },
-{ RET_AVAILABLE_WITH_LINUX_ONLY, "This function is available under Linux only", VS_HIDDEN },
-{ RET_UNKNOWN_BUG, "The error occured is not yet known", VS_VISIBLE },
-{ RET_PRINTLEVEL_CHANGED, "Print level changed", VS_VISIBLE },
-{ RET_NOT_YET_IMPLEMENTED, "Requested function is not yet implemented.", VS_VISIBLE },
-/* Indexlist */
-{ RET_INDEXLIST_MUST_BE_REORDERD, "Index list has to be reordered", VS_VISIBLE },
-{ RET_INDEXLIST_EXCEEDS_MAX_LENGTH, "Index list exceeds its maximal physical length", VS_VISIBLE },
-{ RET_INDEXLIST_CORRUPTED, "Index list corrupted", VS_VISIBLE },
-{ RET_INDEXLIST_OUTOFBOUNDS, "Physical index is out of bounds", VS_VISIBLE },
-{ RET_INDEXLIST_ADD_FAILED, "Adding indices from another index set failed", VS_VISIBLE },
-{ RET_INDEXLIST_INTERSECT_FAILED, "Intersection with another index set failed", VS_VISIBLE },
-/* SubjectTo / Bounds / Constraints */
-{ RET_INDEX_ALREADY_OF_DESIRED_STATUS, "Index is already of desired status", VS_VISIBLE },
-{ RET_SWAPINDEX_FAILED, "Cannot swap between different indexsets", VS_VISIBLE },
-{ RET_ADDINDEX_FAILED, "Adding index to index set failed", VS_VISIBLE },
-{ RET_NOTHING_TO_DO, "Nothing to do", VS_VISIBLE },
-{ RET_SETUP_BOUND_FAILED, "Setting up bound index failed", VS_VISIBLE },
-{ RET_SETUP_CONSTRAINT_FAILED, "Setting up constraint index failed", VS_VISIBLE },
-{ RET_MOVING_BOUND_FAILED, "Moving bound between index sets failed", VS_VISIBLE },
-{ RET_MOVING_CONSTRAINT_FAILED, "Moving constraint between index sets failed", VS_VISIBLE },
-/* QProblem */
-{ RET_QP_ALREADY_INITIALISED, "QProblem has already been initialised", VS_VISIBLE },
-{ RET_NO_INIT_WITH_STANDARD_SOLVER, "Initialisation via extern QP solver is not yet implemented", VS_VISIBLE },
-{ RET_RESET_FAILED, "Reset failed", VS_VISIBLE },
-{ RET_INIT_FAILED, "Initialisation failed", VS_VISIBLE },
-{ RET_INIT_FAILED_TQ, "Initialisation failed due to TQ factorisation", VS_VISIBLE },
-{ RET_INIT_FAILED_CHOLESKY, "Initialisation failed due to Cholesky decomposition", VS_VISIBLE },
-{ RET_INIT_FAILED_HOTSTART, "Initialisation failed! QP could not be solved!", VS_VISIBLE },
-{ RET_INIT_FAILED_INFEASIBILITY, "Initial QP could not be solved due to infeasibility!", VS_VISIBLE },
-{ RET_INIT_FAILED_UNBOUNDEDNESS, "Initial QP could not be solved due to unboundedness!", VS_VISIBLE },
-{ RET_INIT_SUCCESSFUL, "Initialisation done", VS_VISIBLE },
-{ RET_OBTAINING_WORKINGSET_FAILED, "Failed to obtain working set for auxiliary QP", VS_VISIBLE },
-{ RET_SETUP_WORKINGSET_FAILED, "Failed to setup working set for auxiliary QP", VS_VISIBLE },
-{ RET_SETUP_AUXILIARYQP_FAILED, "Failed to setup auxiliary QP for initialised homotopy", VS_VISIBLE },
-{ RET_NO_EXTERN_SOLVER, "No extern QP solver available", VS_VISIBLE },
-{ RET_QP_UNBOUNDED, "QP is unbounded", VS_VISIBLE },
-{ RET_QP_INFEASIBLE, "QP is infeasible", VS_VISIBLE },
-{ RET_QP_NOT_SOLVED, "Problems occured while solving QP with standard solver", VS_VISIBLE },
-{ RET_QP_SOLVED, "QP successfully solved", VS_VISIBLE },
-{ RET_UNABLE_TO_SOLVE_QP, "Problems occured while solving QP", VS_VISIBLE },
-{ RET_INITIALISATION_STARTED, "Starting problem initialisation...", VS_VISIBLE },
-{ RET_HOTSTART_FAILED, "Unable to perform homotopy due to internal error", VS_VISIBLE },
-{ RET_HOTSTART_FAILED_TO_INIT, "Unable to initialise problem", VS_VISIBLE },
-{ RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, "Unable to perform homotopy as previous QP is not solved", VS_VISIBLE },
-{ RET_ITERATION_STARTED, "Iteration", VS_VISIBLE },
-{ RET_SHIFT_DETERMINATION_FAILED, "Determination of shift of the QP data failed", VS_VISIBLE },
-{ RET_STEPDIRECTION_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
-{ RET_STEPLENGTH_DETERMINATION_FAILED, "Determination of step direction failed", VS_VISIBLE },
-{ RET_OPTIMAL_SOLUTION_FOUND, "Optimal solution of neighbouring QP found", VS_VISIBLE },
-{ RET_HOMOTOPY_STEP_FAILED, "Unable to perform homotopy step", VS_VISIBLE },
-{ RET_HOTSTART_STOPPED_INFEASIBILITY, "Premature homotopy termination because QP is infeasible", VS_VISIBLE },
-{ RET_HOTSTART_STOPPED_UNBOUNDEDNESS, "Premature homotopy termination because QP is unbounded", VS_VISIBLE },
-{ RET_WORKINGSET_UPDATE_FAILED, "Unable to update working sets according to initial guesses", VS_VISIBLE },
-{ RET_MAX_NWSR_REACHED, "Maximum number of working set recalculations performed", VS_VISIBLE },
-{ RET_CONSTRAINTS_NOT_SPECIFIED, "Problem does comprise constraints! You have to specify new constraints' bounds", VS_VISIBLE },
-{ RET_INVALID_FACTORISATION_FLAG, "Invalid factorisation flag", VS_VISIBLE },
-{ RET_UNABLE_TO_SAVE_QPDATA, "Unable to save QP data", VS_VISIBLE },
-{ RET_STEPDIRECTION_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
-{ RET_STEPDIRECTION_FAILED_CHOLESKY, "Abnormal termination due to Cholesky factorisation", VS_VISIBLE },
-{ RET_CYCLING_DETECTED, "Cycling detected", VS_VISIBLE },
-{ RET_CYCLING_NOT_RESOLVED, "Cycling cannot be resolved, QP is probably infeasible", VS_VISIBLE },
-{ RET_CYCLING_RESOLVED, "Cycling probably resolved", VS_VISIBLE },
-{ RET_STEPSIZE, "", VS_VISIBLE },
-{ RET_STEPSIZE_NONPOSITIVE, "", VS_VISIBLE },
-{ RET_SETUPSUBJECTTOTYPE_FAILED, "Setup of SubjectToTypes failed", VS_VISIBLE },
-{ RET_ADDCONSTRAINT_FAILED, "Addition of constraint to working set failed", VS_VISIBLE },
-{ RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, "Addition of constraint to working set failed", VS_VISIBLE },
-{ RET_ADDBOUND_FAILED, "Addition of bound to working set failed", VS_VISIBLE },
-{ RET_ADDBOUND_FAILED_INFEASIBILITY, "Addition of bound to working set failed", VS_VISIBLE },
-{ RET_REMOVECONSTRAINT_FAILED, "Removal of constraint from working set failed", VS_VISIBLE },
-{ RET_REMOVEBOUND_FAILED, "Removal of bound from working set failed", VS_VISIBLE },
-{ RET_REMOVE_FROM_ACTIVESET, "Removing from active set:", VS_VISIBLE },
-{ RET_ADD_TO_ACTIVESET, "Adding to active set:", VS_VISIBLE },
-{ RET_REMOVE_FROM_ACTIVESET_FAILED, "Removing from active set failed", VS_VISIBLE },
-{ RET_ADD_TO_ACTIVESET_FAILED, "Adding to active set failed", VS_VISIBLE },
-{ RET_CONSTRAINT_ALREADY_ACTIVE, "Constraint is already active", VS_VISIBLE },
-{ RET_ALL_CONSTRAINTS_ACTIVE, "All constraints are active, no further constraint can be added", VS_VISIBLE },
-{ RET_LINEARLY_DEPENDENT, "New bound/constraint is linearly dependent", VS_VISIBLE },
-{ RET_LINEARLY_INDEPENDENT, "New bound/constraint is linearly independent", VS_VISIBLE },
-{ RET_LI_RESOLVED, "Linear independence of active contraint matrix successfully resolved", VS_VISIBLE },
-{ RET_ENSURELI_FAILED, "Failed to ensure linear indepence of active contraint matrix", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_TQ, "Abnormal termination due to TQ factorisation", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_NOINDEX, "No index found, QP is probably infeasible", VS_VISIBLE },
-{ RET_ENSURELI_FAILED_CYCLING, "Cycling detected, QP is probably infeasible", VS_VISIBLE },
-{ RET_BOUND_ALREADY_ACTIVE, "Bound is already active", VS_VISIBLE },
-{ RET_ALL_BOUNDS_ACTIVE, "All bounds are active, no further bound can be added", VS_VISIBLE },
-{ RET_CONSTRAINT_NOT_ACTIVE, "Constraint is not active", VS_VISIBLE },
-{ RET_BOUND_NOT_ACTIVE, "Bound is not active", VS_VISIBLE },
-{ RET_HESSIAN_NOT_SPD, "Projected Hessian matrix not positive definite", VS_VISIBLE },
-{ RET_MATRIX_SHIFT_FAILED, "Unable to update matrices or to transform vectors", VS_VISIBLE },
-{ RET_MATRIX_FACTORISATION_FAILED, "Unable to calculate new matrix factorisations", VS_VISIBLE },
-{ RET_PRINT_ITERATION_FAILED, "Unable to print information on current iteration", VS_VISIBLE },
-{ RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, "No global message output file initialised", VS_VISIBLE },
-/* Utils */
-{ RET_UNABLE_TO_OPEN_FILE, "Unable to open file", VS_VISIBLE },
-{ RET_UNABLE_TO_WRITE_FILE, "Unable to write into file", VS_VISIBLE },
-{ RET_UNABLE_TO_READ_FILE, "Unable to read from file", VS_VISIBLE },
-{ RET_FILEDATA_INCONSISTENT, "File contains inconsistent data", VS_VISIBLE },
-/* SolutionAnalysis */
-{ RET_NO_SOLUTION, "QP solution does not satisfy KKT optimality conditions", VS_VISIBLE },
-{ RET_INACCURATE_SOLUTION, "KKT optimality conditions not satisfied to sufficient accuracy", VS_VISIBLE },
-{ TERMINAL_LIST_ELEMENT, "", VS_HIDDEN } /* IMPORTANT: Terminal list element! */
-};
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( ) :	errorVisibility( VS_VISIBLE ),
-										warningVisibility( VS_VISIBLE ),
-										infoVisibility( VS_VISIBLE ),
-										outputFile( myStdout ),
-										errorCount( 0 )
-{
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( myFILE* _outputFile ) :
-										errorVisibility( VS_VISIBLE ),
-										warningVisibility( VS_VISIBLE ),
-										infoVisibility( VS_VISIBLE ),
-										outputFile( _outputFile ),
-										errorCount( 0 )
-{
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling(	VisibilityStatus _errorVisibility,
-									VisibilityStatus _warningVisibility,
-		 							VisibilityStatus _infoVisibility
-									) :
-										errorVisibility( _errorVisibility ),
-										warningVisibility( _warningVisibility ),
-										infoVisibility( _infoVisibility ),
-										outputFile( myStderr ),
-										errorCount( 0 )
-{
-}
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( 	myFILE* _outputFile,
-									VisibilityStatus _errorVisibility,
-									VisibilityStatus _warningVisibility,
-		 							VisibilityStatus _infoVisibility
-									) :
-										errorVisibility( _errorVisibility ),
-										warningVisibility( _warningVisibility ),
-										infoVisibility( _infoVisibility ),
-										outputFile( _outputFile ),
-										errorCount( 0 )
-{
-}
-
-
-
-/*
- *	M e s s a g e H a n d l i n g
- */
-MessageHandling::MessageHandling( const MessageHandling& rhs ) :
-										errorVisibility( rhs.errorVisibility ),
-										warningVisibility( rhs.warningVisibility ),
-										infoVisibility( rhs.infoVisibility ),
-										outputFile( rhs.outputFile ),
-										errorCount( rhs.errorCount )
-{
-}
-
-
-/*
- *	~ M e s s a g e H a n d l i n g
- */
-MessageHandling::~MessageHandling( )
-{
-	#ifdef PC_DEBUG
-	if ( outputFile != 0 )
-		fclose( outputFile );
-	#endif
-}
-
-
-/*
- *	o p e r a t o r =
- */
-MessageHandling& MessageHandling::operator=( const MessageHandling& rhs )
-{
-	if ( this != &rhs )
-	{
-		errorVisibility = rhs.errorVisibility;
-		warningVisibility = rhs.warningVisibility;
-		infoVisibility = rhs.infoVisibility;
-		outputFile = rhs.outputFile;
-		errorCount = rhs.errorCount;
-	}
-
-	return *this;
-}
-
-
-/*
- *	t h r o w E r r o r
- */
-returnValue MessageHandling::throwError(
-	returnValue Enumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
-	)
-{
-	/* consistency check */
-	if ( Enumber <= SUCCESSFUL_RETURN )
-		return throwError( RET_ERROR_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if error shall be displayed. */
-	if ( errorVisibility == VS_VISIBLE )
-		return throwMessage( Enumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"ERROR" );
-	else
-		return Enumber;
-}
-
-
-/*
- *	t h r o w W a r n i n g
- */
-returnValue MessageHandling::throwWarning(
-	returnValue Wnumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
-  	)
-{
-	/* consistency check */
-  	if ( Wnumber <= SUCCESSFUL_RETURN )
-		return throwError( RET_WARNING_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if warning shall be displayed. */
-	if ( warningVisibility == VS_VISIBLE )
-		return throwMessage( Wnumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"WARNING" );
-  	else
-  		return Wnumber;
-}
-
-
-/*
- *	t h r o w I n f o
- */
-returnValue MessageHandling::throwInfo(
-  	returnValue Inumber,
-	const char* additionaltext,
-  	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus
- 	)
-{
-	/* consistency check */
-	if ( Inumber < SUCCESSFUL_RETURN )
-		return throwError( RET_INFO_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-
-	/* Call to common throwMessage function if info shall be displayed. */
-	if ( infoVisibility == VS_VISIBLE )
-		return throwMessage( Inumber,additionaltext,functionname,filename,linenumber,localVisibilityStatus,"INFO" );
-	else
-		return Inumber;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue MessageHandling::reset( )
-{
-	setErrorVisibilityStatus(   VS_VISIBLE );
-	setWarningVisibilityStatus( VS_VISIBLE );
-	setInfoVisibilityStatus(    VS_VISIBLE );
-
-	setOutputFile( myStderr );
-	setErrorCount( 0 );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	l i s t A l l M e s s a g e s
- */
-returnValue MessageHandling::listAllMessages( )
-{
-	#ifdef PC_DEBUG
-	int keypos = 0;
-	char myPrintfString[160];
-
-	/* Run through whole returnValueList and print each item. */
-	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
-	{
-		sprintf( myPrintfString," %d - %s \n",keypos,returnValueList[keypos].data );
-		myPrintf( myPrintfString );
-
-		++keypos;
-	}
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-
-#ifdef PC_DEBUG  /* Re-define throwMessage function for embedded code! */
-
-/*
- *	t h r o w M e s s a g e
- */
-returnValue MessageHandling::throwMessage(
-	returnValue RETnumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus,
-	const char* RETstring
- 	)
-{
-	int i;
-
-	int keypos = 0;
-	char myPrintfString[160];
-
-	/* 1) Determine number of whitespace for output. */
-	char whitespaces[41];
-	int numberOfWhitespaces = (errorCount-1)*2;
-
-	if ( numberOfWhitespaces < 0 )
-		numberOfWhitespaces = 0;
-
-	if ( numberOfWhitespaces > 40 )
-		numberOfWhitespaces = 40;
-
-	for( i=0; i<numberOfWhitespaces; ++i )
-		whitespaces[i] = ' ';
-	whitespaces[numberOfWhitespaces] = '\0';
-
-	/* 2) Find error/warning/info in list. */
-	while ( returnValueList[keypos].key != TERMINAL_LIST_ELEMENT )
-	{
-		if ( returnValueList[keypos].key == RETnumber )
-			break;
-		else
-			++keypos;
-	}
-
-	if ( returnValueList[keypos].key == TERMINAL_LIST_ELEMENT )
-	{
-		throwError( RET_EWI_UNDEFINED,0,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		return RETnumber;
-	}
-
-	/* 3) Print error/warning/info. */
-	if ( ( returnValueList[keypos].globalVisibilityStatus == VS_VISIBLE ) && ( localVisibilityStatus == VS_VISIBLE ) )
-	{
-		if ( errorCount > 0 )
-		{
-			sprintf( myPrintfString,"%s->", whitespaces );
-			myPrintf( myPrintfString );
-		}
-
-		if ( additionaltext == 0 )
-		{
-			sprintf(	myPrintfString,"%s (%s, %s:%d): \t%s\n",
-						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data
-						);
-			myPrintf( myPrintfString );
-		}
-		else
-		{
-			sprintf(	myPrintfString,"%s (%s, %s:%d): \t%s %s\n",
-						RETstring,functionname,filename,(int)linenumber,returnValueList[keypos].data,additionaltext
-						);
-			myPrintf( myPrintfString );
-		}
-
-		/* take care of proper indention for subsequent error messages */
-		if ( RETstring[0] == 'E' )
-		{
-			++errorCount;
-		}
-		else
-		{
-			if ( errorCount > 0 )
-				myPrintf( "\n" );
-			errorCount = 0;
-		}
-	}
-
-	return RETnumber;
-}
-
-#else  /* = PC_DEBUG not defined */
-
-/*
- *	t h r o w M e s s a g e
- */
-returnValue MessageHandling::throwMessage(
-	returnValue RETnumber,
-	const char* additionaltext,
-	const char* functionname,
-	const char* filename,
-	const unsigned long linenumber,
-	VisibilityStatus localVisibilityStatus,
-	const char* RETstring
- 	)
-{
-	/* DUMMY CODE FOR PRETENDING USE OF ARGUMENTS
-	 * FOR SUPPRESSING COMPILER WARNINGS! */
-	int i = 0;
-	if ( additionaltext == 0 ) i++;
-	if ( functionname == 0 ) i++;
-	if ( filename == 0 ) i++;
-	if ( linenumber == 0 ) i++;
-	if ( localVisibilityStatus == VS_VISIBLE ) i++;
-	if ( RETstring == 0 ) i++;
-	/* END OF DUMMY CODE */
-
-	return RETnumber;
-}
-
-#endif  /* PC_DEBUG */
-
-
-
-/*****************************************************************************
- *  G L O B A L  M E S S A G E  H A N D L E R                                *
- *****************************************************************************/
-
-
-/** Global message handler for all qpOASES modules.*/
-MessageHandling globalMessageHandler( myStderr,VS_VISIBLE,VS_VISIBLE,VS_VISIBLE );
-
-
-/*
- *	g e t G l o b a l M e s s a g e H a n d l e r
- */
-MessageHandling* getGlobalMessageHandler( )
-{
-	return &globalMessageHandler;
-}
-
-const char* MessageHandling::getErrorString(int error)
-{
-	return returnValueList[ error ].data;
-}
-
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/MessageHandling.ipp b/phonelibs/qpoases/SRC/MessageHandling.ipp
deleted file mode 100644
index 033957b28..000000000
--- a/phonelibs/qpoases/SRC/MessageHandling.ipp
+++ /dev/null
@@ -1,137 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/MessageHandling.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the MessageHandling class. 
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
- 
-/*
- *	g e t E r r o r V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getErrorVisibilityStatus( ) const
-{
- 	return errorVisibility;
-}
-
-
-/*
- *	g e t W a r n i n g V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getWarningVisibilityStatus( ) const
-{
- 	return warningVisibility;
-}
-
-
-/*
- *	g e t I n f o V i s i b i l i t y S t a t u s
- */
-inline VisibilityStatus MessageHandling::getInfoVisibilityStatus( ) const
-{
- 	return infoVisibility;
-}
-
-
-/*
- *	g e t O u t p u t F i l e
- */
-inline myFILE* MessageHandling::getOutputFile( ) const
-{
- 	return outputFile;
-}
-
-
-/*
- *	g e t E r r o r C o u n t
- */
-inline int MessageHandling::getErrorCount( ) const
-{
- 	return errorCount;
-}
-
-
-/*
- *	s e t E r r o r V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setErrorVisibilityStatus( VisibilityStatus _errorVisibility ) 
-{
- 	errorVisibility = _errorVisibility;
-}
-
-
-/*
- *	s e t W a r n i n g V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setWarningVisibilityStatus( VisibilityStatus _warningVisibility ) 
-{
- 	warningVisibility = _warningVisibility;
-}
-
-
-/*
- *	s e t I n f o V i s i b i l i t y S t a t u s
- */
-inline void MessageHandling::setInfoVisibilityStatus( VisibilityStatus _infoVisibility ) 
-{
- 	infoVisibility = _infoVisibility;
-}
-
-
-/*
- *	s e t O u t p u t F i l e
- */
-inline void MessageHandling::setOutputFile( myFILE* _outputFile ) 
-{
- 	outputFile = _outputFile;
-}
-
-
-/*
- *	s e t E r r o r C o u n t
- */
-inline returnValue MessageHandling::setErrorCount( int _errorCount )
-{
-	if ( _errorCount >= 0 ) 	
-	{
-		errorCount = _errorCount;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return RET_INVALID_ARGUMENTS;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/QProblem.cpp b/phonelibs/qpoases/SRC/QProblem.cpp
deleted file mode 100644
index 75e2f324d..000000000
--- a/phonelibs/qpoases/SRC/QProblem.cpp
+++ /dev/null
@@ -1,3867 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/QProblem.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the QProblem class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-#include <QProblem.hpp>
-
-#include <stdio.h>
-
-void printmatrix2(char *name, double *A, int m, int n) {
-  int i, j;
-
-  printf("%s = [...\n", name);
-  for (i = 0; i < m; i++) {
-    for (j = 0; j < n; j++)
-        printf("  % 9.4f", A[i*n+j]);
-    printf(",\n");
-  }
-  printf("];\n");
-}
-
-//#define __PERFORM_KKT_TEST__
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( ) : QProblemB( )
-{
-	constraints.init( 0 );
-
-	sizeT = 0;
-
-	cyclingManager.init( 0,0 );
-}
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( int _nV, int _nC ) : QProblemB( _nV )
-{
-	/* consistency checks */
-	if ( _nV <= 0 )
-		_nV = 1;
-
-	if ( _nC < 0 )
-	{
-		_nC = 0;
-		THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	constraints.init( _nC );
-
-
-	sizeT = _nC;
-	if ( _nC > _nV )
-		sizeT = _nV;
-
-	cyclingManager.init( _nV,_nC );
-}
-
-
-/*
- *	Q P r o b l e m
- */
-QProblem::QProblem( const QProblem& rhs ) :	QProblemB( rhs )
-{
-	int i, j;
-
-	int _nV = rhs.bounds.getNV( );
-	int _nC = rhs.constraints.getNC( );
-
-	for( i=0; i<_nC; ++i )
-		for( j=0; j<_nV; ++j )
-			A[i*NVMAX + j] = rhs.A[i*NVMAX + j];
-
-	for( i=0; i<_nC; ++i )
-		lbA[i] = rhs.lbA[i];
-
-	for( i=0; i<_nC; ++i )
-			ubA[i] = rhs.ubA[i];
-
-	constraints = rhs.constraints;
-
-	for( i=0; i<(_nV+_nC); ++i )
-		y[i] = rhs.y[i];
-
-
-	sizeT = rhs.sizeT;
-
-	for( i=0; i<sizeT; ++i )
-		for( j=0; j<sizeT; ++j )
-			T[i*NVMAX + j] = rhs.T[i*NVMAX + j];
-
-	for( i=0; i<_nV; ++i )
-		for( j=0; j<_nV; ++j )
-			Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];
-
-	for( i=0; i<_nC; ++i )
-		Ax[i] = rhs.Ax[i];
-
-	cyclingManager = rhs.cyclingManager;
-}
-
-
-/*
- *	~ Q P r o b l e m
- */
-QProblem::~QProblem( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-QProblem& QProblem::operator=( const QProblem& rhs )
-{
-	int i, j;
-
-	if ( this != &rhs )
-	{
-		QProblemB::operator=( rhs );
-
-
-		int _nV = rhs.bounds.getNV( );
-		int _nC = rhs.constraints.getNC( );
-
-		for( i=0; i<_nC; ++i )
-			for( j=0; j<_nV; ++j )
-				A[i*NVMAX + j] = rhs.A[i*NVMAX + j];
-
-		for( i=0; i<_nC; ++i )
-			lbA[i] = rhs.lbA[i];
-
-		for( i=0; i<_nC; ++i )
-			ubA[i] = rhs.ubA[i];
-
-		constraints = rhs.constraints;
-
-		for( i=0; i<(_nV+_nC); ++i )
-			y[i] = rhs.y[i];
-
-
-		sizeT = rhs.sizeT;
-
-		for( i=0; i<sizeT; ++i )
-			for( j=0; j<sizeT; ++j )
-				T[i*NVMAX + j] = rhs.T[i*NVMAX + j];
-
-		for( i=0; i<_nV; ++i )
-			for( j=0; j<_nV; ++j )
-				Q[i*NVMAX + j] = rhs.Q[i*NVMAX + j];
-
-		for( i=0; i<_nC; ++i )
-			Ax[i] = rhs.Ax[i];
-
-		cyclingManager = rhs.cyclingManager;
-	}
-
-	return *this;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue QProblem::reset( )
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Reset bounds, Cholesky decomposition and status flags. */
-	if ( QProblemB::reset( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_RESET_FAILED );
-
-	/* 2) Reset constraints. */
-	constraints.init( nC );
-
-	/* 3) Reset TQ factorisation. */
-	for( i=0; i<sizeT; ++i )
-		for( j=0; j<sizeT; ++j )
-			T[i*NVMAX + j] = 0.0;
-
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			Q[i*NVMAX + j] = 0.0;
-
-	/* 4) Reset cycling manager. */
-	if ( cyclingManager.clearCyclingData( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_RESET_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblem::init(	const real_t* const _H, const real_t* const _g, const real_t* const _A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, const real_t* const yOpt, real_t* const cputime
-							)
-{
-	/* 1) Setup QP data. */
-	if (setupQPdata(_H, 0, _g, _A, _lb, _ub, _lbA, _ubA) != SUCCESSFUL_RETURN)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,yOpt,0,0, nWSR,cputime );
-}
-
-returnValue QProblem::init(	const real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A,
-							const real_t* const _lb, const real_t* const _ub,
-							const real_t* const _lbA, const real_t* const _ubA,
-							int& nWSR, const real_t* const yOpt, real_t* const cputime
-							)
-{
-	/* 1) Setup QP data. */
-	if (setupQPdata(_H, _R, _g, _A, _lb, _ub, _lbA, _ubA) != SUCCESSFUL_RETURN)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP( 0,yOpt,0,0, nWSR,cputime );
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblem::hotstart(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
-								const real_t* const lbA_new, const real_t* const ubA_new,
-								int& nWSR, real_t* const cputime
-								)
-{
-	int l;
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	/* I) PREPARATIONS */
-	/* 1) Reset cycling and status flags and increase QP counter. */
-	cyclingManager.clearCyclingData( );
-
-	infeasible = BT_FALSE;
-	unbounded = BT_FALSE;
-
-	++count;
-
-	/* 2) Allocate delta vectors of gradient and (constraints') bounds. */
-	returnValue returnvalue;
-	BooleanType Delta_bC_isZero, Delta_bB_isZero;
-
-	int FR_idx[NVMAX];
-	int FX_idx[NVMAX];
-	int AC_idx[NCMAX_ALLOC];
-	int IAC_idx[NCMAX_ALLOC];
-
-	real_t delta_g[NVMAX];
-	real_t delta_lb[NVMAX];
-	real_t delta_ub[NVMAX];
-	real_t delta_lbA[NCMAX_ALLOC];
-	real_t delta_ubA[NCMAX_ALLOC];
-
-	real_t delta_xFR[NVMAX];
-	real_t delta_xFX[NVMAX];
-	real_t delta_yAC[NCMAX_ALLOC];
-	real_t delta_yFX[NVMAX];
-	real_t delta_Ax[NCMAX_ALLOC];
-
-	int BC_idx;
-	SubjectToStatus BC_status;
-	BooleanType BC_isBound;
-
-	#ifdef PC_DEBUG
-	char messageString[80];
-	#endif
-
-
-	/* II) MAIN HOMOTOPY LOOP */
-	for( l=0; l<nWSR; ++l )
-	{
-		status = QPS_PERFORMINGHOMOTOPY;
-
-		if ( printlevel == PL_HIGH )
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"%d ...",l );
-		  	getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-		}
-
-		/* 1) Setup index arrays. */
-		if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		if ( constraints.getInactive( )->getNumberArray( IAC_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		/* 2) Detemination of shift direction of the gradient and the (constraints') bounds. */
-		returnvalue = hotstart_determineDataShift(  FX_idx, AC_idx,
-													g_new,lbA_new,ubA_new,lb_new,ub_new,
-													delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
-													Delta_bC_isZero, Delta_bB_isZero );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 3) Determination of step direction of X and Y. */
-		returnvalue = hotstart_determineStepDirection(	FR_idx,FX_idx,AC_idx,
-														delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
-														Delta_bC_isZero, Delta_bB_isZero,
-														delta_xFX,delta_xFR,delta_yAC,delta_yFX
-														);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 4) Determination of step length TAU. */
-		returnvalue = hotstart_determineStepLength(	FR_idx,FX_idx,AC_idx,IAC_idx,
-													delta_lbA,delta_ubA,delta_lb,delta_ub,
-													delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
-													BC_idx,BC_status,BC_isBound
-													);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 5) Realisation of the homotopy step. */
-		returnvalue = hotstart_performStep(	FR_idx,FX_idx,AC_idx,IAC_idx,
-											delta_g,delta_lbA,delta_ubA,delta_lb,delta_ub,
-											delta_xFX,delta_xFR,delta_yAC,delta_yFX,delta_Ax,
-											BC_idx,BC_status,BC_isBound
-											);
-
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-
-			/* stop runtime measurement */
-			if ( cputime != 0 )
-					*cputime = getCPUtime( ) - starttime;
-
-			/* optimal solution found? */
-			if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
-			{
-				status = QPS_SOLVED;
-
-				if ( printlevel == PL_HIGH )
-					THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
-
-				#ifdef PC_DEBUG
-	 			if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
-					THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-				#endif
-
-				/* check KKT optimality conditions */
-				return checkKKTconditions( );
-			}
-			else
-			{
-				/* checks for infeasibility... */
-				if ( isInfeasible( ) == BT_TRUE )
-				{
-					status = QPS_HOMOTOPYQPSOLVED;
-					return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
-				}
-
-				/* ...unboundedness... */
-				if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
-					return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
-
-				/* ... and throw unspecific error otherwise */
-				THROWERROR( RET_HOMOTOPY_STEP_FAILED );
-				return returnvalue;
-			}
-		}
-
-		/* 6) Output information of successful QP iteration. */
-		status = QPS_HOMOTOPYQPSOLVED;
-
-		#ifdef PC_DEBUG
-		if ( printIteration( l,BC_idx,BC_status,BC_isBound ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-		#endif
-	}
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-
-	/* if programm gets to here, output information that QP could not be solved
-	 * within the given maximum numbers of working set changes */
-	if ( printlevel == PL_HIGH )
-	{
-		#ifdef PC_DEBUG
-		sprintf( messageString,"(nWSR = %d)",nWSR );
-		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-	/* Finally check KKT optimality conditions. */
-	returnValue returnvalueKKTcheck = checkKKTconditions( );
-
-	if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
-		return returnvalueKKTcheck;
-	else
-		return RET_MAX_NWSR_REACHED;
-}
-
-
-/*
- *	g e t N Z
- */
-int QProblem::getNZ( )
-{
-	/* nZ = nFR - nAC */
-	return bounds.getFree( )->getLength( ) - constraints.getActive( )->getLength( );
-}
-
-
-/*
- *	g e t D u a l S o l u t i o n
- */
-returnValue QProblem::getDualSolution( real_t* const yOpt ) const
-{
-	int i;
-
-	/* return optimal dual solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		for( i=0; i<getNV( )+getNC( ); ++i )
-			yOpt[i] = y[i];
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblem::setupSubjectToType( )
-{
-	int i;
-	int nC = getNC( );
-
-
-	/* I) SETUP SUBJECTTOTYPE FOR BOUNDS */
-	if ( QProblemB::setupSubjectToType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SETUPSUBJECTTOTYPE_FAILED );
-
-
-	/* II) SETUP SUBJECTTOTYPE FOR CONSTRAINTS */
-	/* 1) Check if lower constraints' bounds are present. */
-	constraints.setNoLower( BT_TRUE );
-	for( i=0; i<nC; ++i )
-	{
-		if ( lbA[i] > -INFTY )
-		{
-			constraints.setNoLower( BT_FALSE );
-			break;
-		}
-	}
-
-	/* 2) Check if upper constraints' bounds are present. */
-	constraints.setNoUpper( BT_TRUE );
-	for( i=0; i<nC; ++i )
-	{
-		if ( ubA[i] < INFTY )
-		{
-			constraints.setNoUpper( BT_FALSE );
-			break;
-		}
-	}
-
-	/* 3) Determine implicit equality constraints and unbounded constraints. */
-	int nEC = 0;
-	int nUC = 0;
-
-	for( i=0; i<nC; ++i )
-	{
-		if ( ( lbA[i] < -INFTY + BOUNDTOL ) && ( ubA[i] > INFTY - BOUNDTOL ) )
-		{
-			constraints.setType( i,ST_UNBOUNDED );
-			++nUC;
-		}
-		else
-		{
-			if ( lbA[i] > ubA[i] - BOUNDTOL )
-			{
-				constraints.setType( i,ST_EQUALITY );
-				++nEC;
-			}
-			else
-			{
-				constraints.setType( i,ST_BOUNDED );
-			}
-		}
-	}
-
-	/* 4) Set dimensions of constraints structure. */
-	constraints.setNEC( nEC );
-	constraints.setNUC( nUC );
-	constraints.setNIC( nC - nEC - nUC );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c h o l e s k y D e c o m p o s i t i o n P r o j e c t e d
- */
-returnValue QProblem::setupCholeskyDecompositionProjected( )
-{
-	int i, j, k, ii, kk;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nZ  = getNZ( );
-
-	/* 1) Initialises R with all zeros. */
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			R[i*NVMAX + j] = 0.0;
-
-	/* 2) Calculate Cholesky decomposition of projected Hessian Z'*H*Z. */
-	if ( hessianType == HST_IDENTITY )
-	{
-		/* if Hessian is identity, so is its Cholesky factor. */
-		for( i=0; i<nV; ++i )
-			R[i*NVMAX + i] = 1.0;
-	}
-	else
-	{
-		if ( nZ > 0 )
-		{
-			int FR_idx[NVMAX];
-			if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-#if 0
-			real_t HZ[NVMAX*NVMAX];
-			real_t ZHZ[NVMAX*NVMAX];
-
-			/* calculate H*Z */
-			for ( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-
-				for ( j=0; j<nZ; ++j )
-				{
-					real_t sum = 0.0;
-					for ( k=0; k<nFR; ++k )
-					{
-						kk = FR_idx[k];
-						sum += H[ii*NVMAX + kk] * Q[kk*NVMAX + j];
-					}
-					HZ[i * NVMAX + j] = sum;
-				}
-			}
-
-			/* calculate Z'*H*Z */
-			for ( i=0; i<nZ; ++i )
-				for ( j=0; j<nZ; ++j )
-				{
-					real_t sum = 0.0;
-					for ( k=0; k<nFR; ++k )
-					{
-						kk = FR_idx[k];
-						sum += Q[kk*NVMAX + i] * HZ[k*NVMAX + j];
-					}
-					ZHZ[i * NVMAX + j] = sum;
-				}
-
-			/* R'*R = Z'*H*Z */
-			real_t sum, inv;
-
-			for( i=0; i<nZ; ++i )
-			{
-				/* j == i */
-				sum = ZHZ[i*NVMAX + i];
-
-				for( k=(i-1); k>=0; --k )
-					sum -= R[k*NVMAX + i] * R[k*NVMAX + i];
-
-				if ( sum > 0.0 )
-				{
-					R[i*NVMAX + i] = sqrt( sum );
-					inv = 1.0 / R[i * NVMAX + i];
-				}
-				else
-				{
-					hessianType = HST_SEMIDEF;
-					return THROWERROR( RET_HESSIAN_NOT_SPD );
-				}
-
-				for( j=(i+1); j<nZ; ++j )
-				{
-					sum = ZHZ[j*NVMAX + i];
-
-					for( k=(i-1); k>=0; --k )
-						sum -= R[k*NVMAX + i] * R[k*NVMAX + j];
-
-					R[i*NVMAX + j] = sum * inv;
-				}
-			}
-#else
-			real_t HZ[NVMAX];
-			real_t ZHZ[NVMAX];
-
-			real_t sum, inv;
-			for (j = 0; j < nZ; ++j)
-			{
-				/* Cache one column of Z. */
-				for (i = 0; i < NVMAX; ++i)
-					ZHZ[i] = Q[i * NVMAX + j];
-
-				/* Create one column of the product H * Z. */
-				for (i = 0; i < nFR; ++i)
-				{
-					ii = FR_idx[i];
-
-					sum = 0.0;
-					for (k = 0; k < nFR; ++k)
-					{
-						kk = FR_idx[k];
-						sum += H[ii * NVMAX + kk] * ZHZ[kk];
-					}
-					HZ[ii] = sum;
-				}
-
-				/* Create one column of the product Z^T * H * Z. */
-				for (i = j; i < nZ; ++i)
-					ZHZ[ i ] = 0.0;
-
-				for (k = 0; k < nFR; ++k)
-				{
-					kk = FR_idx[k];
-					real_t q = HZ[kk];
-					for (i = j; i < nZ; ++i)
-					{
-						ZHZ[i] += Q[kk * NVMAX + i] * q;
-					}
-				}
-
-				/* Use the computed column to update the factorization. */
-				/* j == i */
-				sum = ZHZ[j];
-
-				for (k = (j - 1); k >= 0; --k)
-					sum -= R[k * NVMAX + j] * R[k * NVMAX + j];
-
-				if (sum > 0.0)
-				{
-					R[j * NVMAX + j] = sqrt(sum);
-					inv = 1.0 / R[j * NVMAX + j];
-				}
-				else
-				{
-					hessianType = HST_SEMIDEF;
-					return THROWERROR( RET_HESSIAN_NOT_SPD );
-				}
-
-				for (i = (j + 1); i < nZ; ++i)
-				{
-					sum = ZHZ[i];
-
-					for (k = (j - 1); k >= 0; --k)
-						sum -= R[k * NVMAX + j] * R[k * NVMAX + i];
-
-					R[j * NVMAX + i] = sum * inv;
-				}
-			}
-#endif
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p T Q f a c t o r i s a t i o n
- */
-returnValue QProblem::setupTQfactorisation( )
-{
-	int i, j, ii;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-	/* 1) Set Q to unity matrix. */
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			Q[i*NVMAX + j] = 0.0;
-
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		Q[ii*NVMAX + i] = 1.0;
-	}
-
- 	/* 2) Set T to zero matrix. */
-	for( i=0; i<sizeT; ++i )
-		for( j=0; j<sizeT; ++j )
-			T[i*NVMAX + j] = 0.0;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s o l v e I n i t i a l Q P
- */
-returnValue QProblem::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
-										const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
-										int& nWSR, real_t* const cputime
-										)
-{
-	int i;
-
-	/* some definitions */
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	status = QPS_NOTINITIALISED;
-
-	/* I) ANALYSE QP DATA: */
-	/* 1) Check if Hessian happens to be the identity matrix. */
-	if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup type of bounds and constraints (i.e. unbounded, implicitly fixed etc.). */
-	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 3) Initialise cycling manager. */
-	cyclingManager.clearCyclingData( );
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
-	/* 1) Setup bounds and constraints data structure. */
-	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	if ( constraints.setupAllInactive( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
-	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 3) Obtain linear independent working set for auxiliary QP. */
-
-	static Bounds auxiliaryBounds;
-
-	auxiliaryBounds.init( nV );
-
-	static Constraints auxiliaryConstraints;
-
-	auxiliaryConstraints.init( nC );
-
-	if ( obtainAuxiliaryWorkingSet(	xOpt,yOpt,guessedBounds,guessedConstraints,
-									&auxiliaryBounds,&auxiliaryConstraints ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 4) Setup working set of auxiliary QP and setup matrix factorisations. */
-	if ( setupTQfactorisation( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED_TQ );
-
-	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	if ( ( getNAC( ) + getNFX( ) ) == 0 )
-	{
-		/* Factorise full Hessian if no bounds/constraints are active. */
-		if (hasCholesky == BT_FALSE)
-			if ( setupCholeskyDecomposition( ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_INIT_FAILED_CHOLESKY );
-		/* ... else we use user provided Cholesky factorization. At the moment
-		 * we can do that only for cold-started solver. */
-	}
-	else
-	{
-		/* Factorise projected Hessian if there active bounds/constraints. */
-		if ( setupCholeskyDecompositionProjected( ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_INIT_FAILED_CHOLESKY );
-		/* TODO: use user-supplied Hessian decomposition. R_Z = R * Z. */
-	}
-
-	/* 5) Store original QP formulation... */
-	real_t g_original[NVMAX];
-	real_t lb_original[NVMAX];
-	real_t ub_original[NVMAX];
-	real_t lbA_original[NCMAX_ALLOC];
-	real_t ubA_original[NCMAX_ALLOC];
-
-	for( i=0; i<nV; ++i )
-	{
-		g_original[i] = g[i];
-		lb_original[i] = lb[i];
-		ub_original[i] = ub[i];
-	}
-
-	for( i=0; i<nC; ++i )
-	{
-		lbA_original[i] = lbA[i];
-		ubA_original[i] = ubA[i];
-	}
-
-	/* ... and setup QP data of an auxiliary QP having an optimal solution
-	 * as specified by the user (or xOpt = yOpt = 0, by default). */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	if ( setupAuxiliaryQPbounds( &auxiliaryBounds,&auxiliaryConstraints,BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-
-	/* III) SOLVE ACTUAL INITIAL QP: */
-	/* Use hotstart method to find the solution of the original initial QP,... */
-	returnValue returnvalue = hotstart( g_original,lb_original,ub_original,lbA_original,ubA_original, nWSR,0 );
-
-
-	/* ... check for infeasibility and unboundedness... */
-	if ( isInfeasible( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
-
-	if ( isUnbounded( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
-
-	/* ... and internal errors. */
-	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED )  &&
-	     ( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
-		return THROWERROR( RET_INIT_FAILED_HOTSTART );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	if ( printlevel == PL_HIGH )
-		THROWINFO( RET_INIT_SUCCESSFUL );
-
-	return returnvalue;
-}
-
-
-/*
- *	o b t a i n A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblem::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
-													const Bounds* const guessedBounds, const Constraints* const guessedConstraints,
-													Bounds* auxiliaryBounds, Constraints* auxiliaryConstraints
-													) const
-{
-	int i = 0;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
-	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( ( auxiliaryConstraints == 0 ) || ( auxiliaryConstraints == guessedConstraints ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	SubjectToStatus guessedStatus;
-
-	/* 2) Setup working set of bounds for auxiliary initial QP. */
-	if ( QProblemB::obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, auxiliaryBounds ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-
-	/* 3) Setup working set of constraints for auxiliary initial QP. */
-	if ( guessedConstraints != 0 )
-	{
-		/* If an initial working set is specific, use it!
-		 * Moreover, add all equality constraints if specified. */
-		for( i=0; i<nC; ++i )
-		{
-			guessedStatus = guessedConstraints->getStatus( i );
-
-			if ( constraints.getType( i ) == ST_EQUALITY )
-			{
-				if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-			else
-			{
-				if ( auxiliaryConstraints->setupConstraint( i,guessedStatus ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-		}
-	}
-	else	/* No initial working set specified. */
-	{
-		/* Obtain initial working set by "clipping". */
-		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				if ( Ax[i] <= lbA[i] + BOUNDTOL )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( Ax[i] >= ubA[i] - BOUNDTOL )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-							return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all equality constraints if specified. */
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* Obtain initial working set in accordance to sign of dual solution vector. */
-		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				if ( yOpt[nV+i] > ZERO )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( yOpt[nV+i] < -ZERO )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all equality constraints if specified. */
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* If xOpt and yOpt are null pointer and no initial working is specified,
-		 * start with empty working set (or implicitly fixed bounds and equality constraints only)
-		 * for auxiliary QP. */
-		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nC; ++i )
-			{
-				/* Only add all equality constraints if specified. */
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryConstraints->setupConstraint( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	s e t u p A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblem::setupAuxiliaryWorkingSet(	const Bounds* const auxiliaryBounds,
-												const Constraints* const auxiliaryConstraints,
-												BooleanType setupAfresh
-												)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	/* consistency checks */
-	if ( auxiliaryBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	if ( auxiliaryConstraints != 0 )
-	{
-		for( i=0; i<nC; ++i )
-			if ( ( constraints.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryConstraints->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-
-	/* I) SETUP CHOLESKY FLAG:
-	 *    Cholesky decomposition shall only be updated if working set
-	 *    shall be updated (i.e. NOT setup afresh!) */
-	BooleanType updateCholesky;
-	if ( setupAfresh == BT_TRUE )
-		updateCholesky = BT_FALSE;
-	else
-		updateCholesky = BT_TRUE;
-
-
-	/* II) REMOVE FORMERLY ACTIVE (CONSTRAINTS') BOUNDS (IF NECESSARY): */
-	if ( setupAfresh == BT_FALSE )
-	{
-		/* 1) Remove all active constraints that shall be inactive AND
-		*    all active constraints that are active at the wrong bound. */
-		for( i=0; i<nC; ++i )
-		{
-			if ( ( constraints.getStatus( i ) == ST_LOWER ) && ( auxiliaryConstraints->getStatus( i ) != ST_LOWER ) )
-				if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( constraints.getStatus( i ) == ST_UPPER ) && ( auxiliaryConstraints->getStatus( i ) != ST_UPPER ) )
-				if ( removeConstraint( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-
-		/* 2) Remove all active bounds that shall be inactive AND
-		*    all active bounds that are active at the wrong bound. */
-		for( i=0; i<nV; ++i )
-		{
-			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-
-	/* III) ADD NEWLY ACTIVE (CONSTRAINTS') BOUNDS: */
-	/* 1) Add all inactive bounds that shall be active AND
-	 *    all formerly active bounds that have been active at the wrong bound. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
-		{
-			/* Add bound only if it is linearly independent from the current working set. */
-			if ( addBound_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
-			{
-				if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-			}
-		}
-	}
-
-	/* 2) Add all inactive constraints that shall be active AND
-	 *    all formerly active constraints that have been active at the wrong bound. */
-	for( i=0; i<nC; ++i )
-	{
-		if ( ( auxiliaryConstraints->getStatus( i ) == ST_LOWER ) || ( auxiliaryConstraints->getStatus( i ) == ST_UPPER ) )
-		{
-			/* formerly inactive */
-			if ( constraints.getStatus( i ) == ST_INACTIVE )
-			{
-				/* Add constraint only if it is linearly independent from the current working set. */
-				if ( addConstraint_checkLI( i ) == RET_LINEARLY_INDEPENDENT )
-				{
-					if ( addConstraint( i,auxiliaryConstraints->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P s o l u t i o n
- */
-returnValue QProblem::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
-												)
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* Setup primal/dual solution vector for auxiliary initial QP:
-	 * if a null pointer is passed, a zero vector is assigned;
-	 *  old solution vector is kept if pointer to internal solution vevtor is passed. */
-	if ( xOpt != 0 )
-	{
-		if ( xOpt != x )
-			for( i=0; i<nV; ++i )
-				x[i] = xOpt[i];
-
-		for ( j=0; j<nC; ++j )
-		{
-			Ax[j] = 0.0;
-
-			for( i=0; i<nV; ++i )
-				Ax[j] += A[j*NVMAX + i] * x[i];
-		}
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			x[i] = 0.0;
-
-		for ( j=0; j<nC; ++j )
-			Ax[j] = 0.0;
-	}
-
-	if ( yOpt != 0 )
-	{
-		if ( yOpt != y )
-			for( i=0; i<nV+nC; ++i )
-				y[i] = yOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV+nC; ++i )
-			y[i] = 0.0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P g r a d i e n t
- */
-returnValue QProblem::setupAuxiliaryQPgradient( )
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* Setup gradient vector: g = -H*x + [Id A]'*[yB yC]. */
-	for ( i=0; i<nV; ++i )
-	{
-		/* Id'*yB */
-		g[i] = y[i];
-
-		/* A'*yC */
-		for ( j=0; j<nC; ++j )
-			g[i] += A[j*NVMAX + i] * y[nV+j];
-
-		/* -H*x */
-		for ( j=0; j<nV; ++j )
-			g[i] -= H[i*NVMAX + j] * x[j];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P b o u n d s
- */
-returnValue QProblem::setupAuxiliaryQPbounds(	const Bounds* const auxiliaryBounds,
-												const Constraints* const auxiliaryConstraints,
-												BooleanType useRelaxation
-												)
-{
-	int i;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Setup bound vectors. */
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( bounds.getType( i ) == ST_EQUALITY )
-					{
-						lb[i] = x[i];
-						ub[i] = x[i];
-					}
-					else
-					{
-						/* If a bound is inactive although it was supposed to be
-						* active by the auxiliaryBounds, it could not be added
-						* due to linear dependence. Thus set it "strongly inactive". */
-						if ( auxiliaryBounds->getStatus( i ) == ST_LOWER )
-							lb[i] = x[i];
-						else
-							lb[i] = x[i] - BOUNDRELAXATION;
-
-						if ( auxiliaryBounds->getStatus( i ) == ST_UPPER )
-							ub[i] = x[i];
-						else
-							ub[i] = x[i] + BOUNDRELAXATION;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lb[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					ub[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ub[i] = x[i] + BOUNDRELAXATION;
-				}
-				break;
-
-			case ST_UPPER:
-				ub[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					lb[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lb[i] = x[i] - BOUNDRELAXATION;
-				}
-				break;
-
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-	}
-
-	/* 2) Setup constraints vectors. */
-	for ( i=0; i<nC; ++i )
-	{
-		switch ( constraints.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( constraints.getType( i ) == ST_EQUALITY )
-					{
-						lbA[i] = Ax[i];
-						ubA[i] = Ax[i];
-					}
-					else
-					{
-						/* If a constraint is inactive although it was supposed to be
-						* active by the auxiliaryConstraints, it could not be added
-						* due to linear dependence. Thus set it "strongly inactive". */
-						if ( auxiliaryConstraints->getStatus( i ) == ST_LOWER )
-							lbA[i] = Ax[i];
-						else
-							lbA[i] = Ax[i] - BOUNDRELAXATION;
-
-						if ( auxiliaryConstraints->getStatus( i ) == ST_UPPER )
-							ubA[i] = Ax[i];
-						else
-							ubA[i] = Ax[i] + BOUNDRELAXATION;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lbA[i] = Ax[i];
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					ubA[i] = Ax[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ubA[i] = Ax[i] + BOUNDRELAXATION;
-				}
-				break;
-
-			case ST_UPPER:
-				ubA[i] = Ax[i];
-				if ( constraints.getType( i ) == ST_EQUALITY )
-				{
-					lbA[i] = Ax[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lbA[i] = Ax[i] - BOUNDRELAXATION;
-				}
-				break;
-
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d C o n s t r a i n t
- */
-returnValue QProblem::addConstraint(	int number, SubjectToStatus C_status,
-										BooleanType updateCholesky
-										)
-{
-	int i, j, ii;
-
-	/* consistency checks */
-	if ( constraints.getStatus( number ) != ST_INACTIVE )
-		return THROWERROR( RET_CONSTRAINT_ALREADY_ACTIVE );
-
-	if ( ( constraints.getNC( ) - getNAC( ) ) == constraints.getNUC( ) )
-		return THROWERROR( RET_ALL_CONSTRAINTS_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-
-	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
-	 *    i.e. remove a constraint or bound if linear dependence occurs. */
-	/* check for LI only if Cholesky decomposition shall be updated! */
-	if ( updateCholesky == BT_TRUE )
-	{
-		returnValue ensureLIreturnvalue = addConstraint_ensureLI( number,C_status );
-
-		switch ( ensureLIreturnvalue )
-		{
-			case SUCCESSFUL_RETURN:
-				break;
-
-			case RET_LI_RESOLVED:
-				break;
-
-			case RET_ENSURELI_FAILED_NOINDEX:
-				return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );
-
-			case RET_ENSURELI_FAILED_CYCLING:
-				return THROWERROR( RET_ADDCONSTRAINT_FAILED_INFEASIBILITY );
-
-			default:
-				return THROWERROR( RET_ENSURELI_FAILED );
-		}
-	}
-
-	/* some definitions */
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDCONSTRAINT_FAILED );
-
-	real_t aFR[NVMAX];
-	real_t wZ[NVMAX];
-	for( i=0; i<nZ; ++i )
-		wZ[i] = 0.0;
-
-
-	/* II) ADD NEW ACTIVE CONSTRAINT TO MATRIX T: */
-	/* 1) Add row [wZ wY] = aFR'*[Z Y] to the end of T: assign aFR. */
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		aFR[i] = A[number*NVMAX + ii];
-	}
-
-	/* calculate wZ */
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		for( j=0; j<nZ; ++j )
-			wZ[j] += aFR[i] * Q[ii*NVMAX + j];
-	}
-
-	/* 2) Calculate wY and store it directly into T. */
-	if ( nAC > 0 )
-	{
-		for( j=0; j<nAC; ++j )
-			T[nAC*NVMAX + tcol+j] = 0.0;
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			for( j=0; j<nAC; ++j )
-				T[nAC*NVMAX + tcol+j] += aFR[i] * Q[ii*NVMAX + nZ+j];
-		}
-	}
-
-
-	real_t c, s;
-
-	if ( nZ > 0 )
-	{
-		/* II) RESTORE TRIANGULAR FORM OF T: */
-		/*     Use column-wise Givens rotations to restore reverse triangular form
-		*      of T, simultanenous change of Q (i.e. Z) and R. */
-		for( j=0; j<nZ-1; ++j )
-		{
-			computeGivens( wZ[j+1],wZ[j], wZ[j+1],wZ[j],c,s );
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
-			}
-
-			if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-			{
-				for( i=0; i<=j+1; ++i )
-					applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
-			}
-		}
-
-		T[nAC*NVMAX + tcol-1] = wZ[nZ-1];
-
-
-		if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-		{
-			/* III) RESTORE TRIANGULAR FORM OF R:
-			 *      Use row-wise Givens rotations to restore upper triangular form of R. */
-			for( i=0; i<nZ-1; ++i )
-			{
-				computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );
-
-				for( j=(1+i); j<(nZ-1); ++j ) /* last column of R is thrown away */
-					applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
-			}
-			/* last column of R is thrown away */
-			for( i=0; i<nZ; ++i )
-				R[i*NVMAX + nZ-1] = 0.0;
-		}
-	}
-
-
-	/* IV) UPDATE INDICES */
-	if ( constraints.moveInactiveToActive( number,C_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDCONSTRAINT_FAILED );
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	a d d C o n s t r a i n t _ c h e c k L I
- */
-returnValue QProblem::addConstraint_checkLI( int number )
-{
-	int i, j, jj;
-	int nFR = getNFR( );
-	int nZ  = getNZ( );
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-	/* Check if constraint <number> is linearly independent from the
-	   the active ones (<=> is element of null space of Afr). */
-	real_t sum;
-
-	for( i=0; i<nZ; ++i )
-	{
-		sum = 0.0;
-		for( j=0; j<nFR; ++j )
-		{
-			jj = FR_idx[j];
-			sum += Q[jj*NVMAX + i] * A[number*NVMAX + jj];
-		}
-
-		if ( getAbs( sum ) > 10.0*EPS )
-			return RET_LINEARLY_INDEPENDENT;
-	}
-
-	return RET_LINEARLY_DEPENDENT;
-}
-
-
-/*
- *	a d d C o n s t r a i n t _ e n s u r e L I
- */
-returnValue QProblem::addConstraint_ensureLI( int number, SubjectToStatus C_status )
-{
-	int i, j, ii, jj;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-
-	/* I) Check if new constraint is linearly independent from the active ones. */
-	returnValue returnvalueCheckLI = addConstraint_checkLI( number );
-
-	if ( returnvalueCheckLI == RET_INDEXLIST_CORRUPTED )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
-		return SUCCESSFUL_RETURN;
-
-
- 	/* II) NEW CONSTRAINT IS LINEARLY DEPENDENT: */
-	/* 1) Determine coefficients of linear combination,
-	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
-	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
-	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	int FX_idx[NVMAX];
-	if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	real_t xiC[NCMAX_ALLOC];
-	real_t xiC_TMP[NCMAX_ALLOC];
-	real_t xiB[NVMAX];
-
-	/* 2) Calculate xiC */
-	if ( nAC > 0 )
-	{
-		if ( C_status == ST_LOWER )
-		{
-			for( i=0; i<nAC; ++i )
-			{
-				xiC_TMP[i] = 0.0;
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					xiC_TMP[i] += Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
-				}
-			}
-		}
-		else
-		{
-			for( i=0; i<nAC; ++i )
-			{
-				xiC_TMP[i] = 0.0;
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					xiC_TMP[i] -= Q[jj*NVMAX + nZ+i] * A[number*NVMAX + jj];
-				}
-			}
-		}
-
-		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_ENSURELI_FAILED_TQ );
-	}
-
-	/* 3) Calculate xiB. */
-	int AC_idx[NCMAX_ALLOC];
-	if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	if ( C_status == ST_LOWER )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			xiB[i] = A[number*NVMAX + ii];
-
-			for( j=0; j<nAC; ++j )
-			{
-				jj = AC_idx[j];
-				xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
-			}
-		}
-	}
-	else
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			xiB[i] = -A[number*NVMAX + ii];
-
-			for( j=0; j<nAC; ++j )
-			{
-				jj = AC_idx[j];
-				xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
-			}
-		}
-	}
-
-
-	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
-	real_t y_min = INFTY * INFTY;
-	int y_min_number = -1;
-	BooleanType y_min_isBound = BT_FALSE;
-
-	/* 1) Constraints. */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		if ( constraints.getStatus( ii ) == ST_LOWER )
-		{
-			if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
-			{
-				if ( y[nV+ii]/xiC[i] < y_min )
-				{
-					y_min = y[nV+ii]/xiC[i];
-					y_min_number = ii;
-				}
-			}
-		}
-		else
-		{
-			if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
-			{
-				if ( y[nV+ii]/xiC[i] < y_min )
-				{
-					y_min = y[nV+ii]/xiC[i];
-					y_min_number = ii;
-				}
-			}
-		}
-	}
-
-	/* 2) Bounds. */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( bounds.getStatus( ii ) == ST_LOWER )
-		{
-			if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
-			{
-				if ( y[ii]/xiB[i] < y_min )
-				{
-					y_min = y[ii]/xiB[i];
-					y_min_number = ii;
-					y_min_isBound = BT_TRUE;
-				}
-			}
-		}
-		else
-		{
-			if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
-			{
-				if ( y[ii]/xiB[i] < y_min )
-				{
-					y_min = y[ii]/xiB[i];
-					y_min_number = ii;
-					y_min_isBound = BT_TRUE;
-				}
-			}
-		}
-	}
-
-	/* setup output preferences */
-	#ifdef PC_DEBUG
-	char messageString[80];
-	VisibilityStatus visibilityStatus;
-
-	if ( printlevel == PL_HIGH )
-		visibilityStatus = VS_VISIBLE;
-	else
-		visibilityStatus = VS_HIDDEN;
-	#endif
-
-
-	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
-	if ( y_min_number >= 0 )
-	{
-		/* 1) Check for cycling due to infeasibility. */
-		if ( ( cyclingManager.getCyclingStatus( number,BT_FALSE ) == CYC_PREV_REMOVED ) &&
-			 ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
-		{
-			infeasible = BT_TRUE;
-
-			return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
-		}
-		else
-		{
-			/* set cycling data */
-			cyclingManager.clearCyclingData( );
-			cyclingManager.setCyclingStatus( number,BT_FALSE, CYC_PREV_ADDED );
-			cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
-		}
-
-		/* 2) Update Lagrange multiplier... */
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] -= y_min * xiC[i];
-		}
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			y[ii] -= y_min * xiB[i];
-		}
-
-		/* ... also for newly active constraint... */
-		if ( C_status == ST_LOWER )
-			y[nV+number] = y_min;
-		else
-			y[nV+number] = -y_min;
-
-		/* ... and for constraint to be removed. */
-		if ( y_min_isBound == BT_TRUE )
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"bound no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[y_min_number] = 0.0;
-		}
-		else
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"constraint no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[nV+y_min_number] = 0.0;
-		}
-	}
-	else
-	{
-		/* no constraint/bound can be removed => QP is infeasible! */
-		infeasible = BT_TRUE;
-
-		return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
-	}
-
-	return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
-}
-
-
-
-/*
- *	a d d B o u n d
- */
-returnValue QProblem::addBound(	int number, SubjectToStatus B_status,
-								BooleanType updateCholesky
-								)
-{
-	int i, j, ii;
-
-	/* consistency checks */
-	if ( bounds.getStatus( number ) != ST_INACTIVE )
-		return THROWERROR( RET_BOUND_ALREADY_ACTIVE );
-
-	if ( getNFR( ) == bounds.getNUV( ) )
-		return THROWERROR( RET_ALL_BOUNDS_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-
-	/* I) ENSURE LINEAR INDEPENDENCE OF THE WORKING SET,
-	 *    i.e. remove a constraint or bound if linear dependence occurs. */
-	/* check for LI only if Cholesky decomposition shall be updated! */
-	if ( updateCholesky == BT_TRUE )
-	{
-		returnValue ensureLIreturnvalue = addBound_ensureLI( number,B_status );
-
-		switch ( ensureLIreturnvalue )
-		{
-			case SUCCESSFUL_RETURN:
-				break;
-
-			case RET_LI_RESOLVED:
-				break;
-
-			case RET_ENSURELI_FAILED_NOINDEX:
-				return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );
-
-			case RET_ENSURELI_FAILED_CYCLING:
-				return THROWERROR( RET_ADDBOUND_FAILED_INFEASIBILITY );
-
-			default:
-				return THROWERROR( RET_ENSURELI_FAILED );
-		}
-	}
-
-
-	/* some definitions */
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-
-	/* I) SWAP INDEXLIST OF FREE VARIABLES:
-	 *    move the variable to be fixed to the end of the list of free variables. */
-	int lastfreenumber = bounds.getFree( )->getLastNumber( );
-	if ( lastfreenumber != number )
-		if ( bounds.swapFree( number,lastfreenumber ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_ADDBOUND_FAILED );
-
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDBOUND_FAILED );
-
-	real_t w[NVMAX];
-
-
-	/* II) ADD NEW ACTIVE BOUND TO TOP OF MATRIX T: */
-	/* 1) add row [wZ wY] = [Z Y](number) at the top of T: assign w */
-	for( i=0; i<nFR; ++i )
-		w[i] = Q[FR_idx[nFR-1]*NVMAX + i];
-
-
-	/* 2) Use column-wise Givens rotations to restore reverse triangular form
-	 *    of the first row of T, simultanenous change of Q (i.e. Z) and R. */
-	real_t c, s;
-
-	for( j=0; j<nZ-1; ++j )
-	{
-		computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
-		}
-
-		if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-		{
-			for( i=0; i<=j+1; ++i )
-				applyGivens( c,s,R[i*NVMAX + 1+j],R[i*NVMAX + j], R[i*NVMAX + 1+j],R[i*NVMAX + j] );
-		}
-	}
-
-
-	if ( nAC > 0 )	  /* ( nAC == 0 ) <=> ( nZ == nFR ) <=> Y and T are empty => nothing to do */
-	{
-		/* store new column a in a temporary vector instead of shifting T one column to the left */
-		real_t tmp[NCMAX_ALLOC];
-		for( i=0; i<nAC; ++i )
-			tmp[i] = 0.0;
-
-		{
-			j = nZ-1;
-
-			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
-			}
-
-			applyGivens( c,s,T[(nAC-1)*NVMAX + tcol],tmp[nAC-1], tmp[nAC-1],T[(nAC-1)*NVMAX + tcol] );
-		}
-
-		for( j=nZ; j<nFR-1; ++j )
-		{
-			computeGivens( w[j+1],w[j], w[j+1],w[j],c,s );
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j], Q[ii*NVMAX + 1+j],Q[ii*NVMAX + j] );
-			}
-
-			for( i=(nFR-2-j); i<nAC; ++i )
-				applyGivens( c,s,T[i*NVMAX + 1+tcol-nZ+j],tmp[i], tmp[i],T[i*NVMAX + 1+tcol-nZ+j] );
-		}
-
-	}
-
-
-	if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-	{
-		/* III) RESTORE TRIANGULAR FORM OF R:
-		 *      use row-wise Givens rotations to restore upper triangular form of R */
-		for( i=0; i<nZ-1; ++i )
-		{
-			computeGivens( R[i*NVMAX + i],R[(1+i)*NVMAX + i], R[i*NVMAX + i],R[(1+i)*NVMAX + i],c,s );
-
-			for( j=(1+i); j<nZ-1; ++j ) /* last column of R is thrown away */
-				applyGivens( c,s,R[i*NVMAX + j],R[(1+i)*NVMAX + j], R[i*NVMAX + j],R[(1+i)*NVMAX + j] );
-		}
-		/* last column of R is thrown away */
-		for( i=0; i<nZ; ++i )
-			R[i*NVMAX + nZ-1] = 0.0;
-	}
-
-
-	/* IV) UPDATE INDICES */
-	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDBOUND_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d B o u n d _ c h e c k L I
- */
-returnValue QProblem::addBound_checkLI( int number )
-{
-	int i;
-
-	/* some definitions */
-	int nZ  = getNZ( );
-
-	/* Check if constraint <number> is linearly independent from the
-	   the active ones (<=> is element of null space of Afr). */
-	for( i=0; i<nZ; ++i )
-	{
-		if ( getAbs( Q[number*NVMAX + i] ) > EPS )
-			return RET_LINEARLY_INDEPENDENT;
-	}
-
-	return RET_LINEARLY_DEPENDENT;
-}
-
-
-/*
- *	a d d B o u n d _ e n s u r e L I
- */
-returnValue QProblem::addBound_ensureLI( int number, SubjectToStatus B_status )
-{
-	int i, j, ii, jj;
-	int nV  = getNV( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-
-	/* I) Check if new constraint is linearly independent from the active ones. */
-	returnValue returnvalueCheckLI = addBound_checkLI( number );
-
-	if ( returnvalueCheckLI == RET_LINEARLY_INDEPENDENT )
-		return SUCCESSFUL_RETURN;
-
-
- 	/* II) NEW BOUND IS LINEARLY DEPENDENT: */
-	/* 1) Determine coefficients of linear combination,
-	 *    cf. M.J. Best. Applied Mathematics and Parallel Computing, chapter:
-	 *    An Algorithm for the Solution of the Parametric Quadratic Programming
-	 *    Problem, pages 57-76. Physica-Verlag, Heidelberg, 1996. */
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	int FX_idx[NVMAX];
-	if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	int AC_idx[NCMAX_ALLOC];
-	if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ENSURELI_FAILED );
-
-	real_t xiC[NCMAX_ALLOC];
-	real_t xiC_TMP[NCMAX_ALLOC];
-	real_t xiB[NVMAX];
-
-	/* 2) Calculate xiC. */
-	if ( nAC > 0 )
-	{
-		if ( B_status == ST_LOWER )
-		{
-			for( i=0; i<nAC; ++i )
-				xiC_TMP[i] = Q[number*NVMAX + nZ+i];
-		}
-		else
-		{
-			for( i=0; i<nAC; ++i )
-				xiC_TMP[i] = -Q[number*NVMAX + nZ+i];
-		}
-
-		if ( backsolveT( xiC_TMP, BT_TRUE, xiC ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_ENSURELI_FAILED_TQ );
-	}
-
-	/* 3) Calculate xiB. */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		xiB[i] = 0.0;
-		for( j=0; j<nAC; ++j )
-		{
-			jj = AC_idx[j];
-			xiB[i] -= A[jj*NVMAX + ii] * xiC[j];
-		}
-	}
-
-
-	/* III) DETERMINE CONSTRAINT/BOUND TO BE REMOVED. */
-	real_t y_min = INFTY * INFTY;
-	int y_min_number = -1;
-	BooleanType y_min_isBound = BT_FALSE;
-
-	/* 1) Constraints. */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		if ( constraints.getStatus( ii ) == ST_LOWER )
-		{
-			if ( ( xiC[i] > ZERO ) && ( y[nV+ii] >= 0.0 ) )
-			{
-				if ( y[nV+ii]/xiC[i] < y_min )
-				{
-					y_min = y[nV+ii]/xiC[i];
-					y_min_number = ii;
-				}
-			}
-		}
-		else
-		{
-			if ( ( xiC[i] < -ZERO ) && ( y[nV+ii] <= 0.0 ) )
-			{
-				if ( y[nV+ii]/xiC[i] < y_min )
-				{
-					y_min = y[nV+ii]/xiC[i];
-					y_min_number = ii;
-				}
-			}
-		}
-	}
-
-	/* 2) Bounds. */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( bounds.getStatus( ii ) == ST_LOWER )
-		{
-			if ( ( xiB[i] > ZERO ) && ( y[ii] >= 0.0 ) )
-			{
-				if ( y[ii]/xiB[i] < y_min )
-				{
-					y_min = y[ii]/xiB[i];
-					y_min_number = ii;
-					y_min_isBound = BT_TRUE;
-				}
-			}
-		}
-		else
-		{
-			if ( ( xiB[i] < -ZERO ) && ( y[ii] <= 0.0 ) )
-			{
-				if ( y[ii]/xiB[i] < y_min )
-				{
-					y_min = y[ii]/xiB[i];
-					y_min_number = ii;
-					y_min_isBound = BT_TRUE;
-				}
-			}
-		}
-	}
-
-	/* setup output preferences */
-	#ifdef PC_DEBUG
-	char messageString[80];
-	VisibilityStatus visibilityStatus;
-
-	if ( printlevel == PL_HIGH )
-		visibilityStatus = VS_VISIBLE;
-	else
-		visibilityStatus = VS_HIDDEN;
-	#endif
-
-
-	/* IV) REMOVE CONSTRAINT/BOUND FOR RESOLVING LINEAR DEPENDENCE: */
-	if ( y_min_number >= 0 )
-	{
-		/* 1) Check for cycling due to infeasibility. */
-		if ( ( cyclingManager.getCyclingStatus( number,BT_TRUE ) == CYC_PREV_REMOVED ) &&
-			 ( cyclingManager.getCyclingStatus( y_min_number,y_min_isBound ) == CYC_PREV_ADDED ) )
-		{
-			infeasible = BT_TRUE;
-
-			return THROWERROR( RET_ENSURELI_FAILED_CYCLING );
-		}
-		else
-		{
-			/* set cycling data */
-			cyclingManager.clearCyclingData( );
-			cyclingManager.setCyclingStatus( number,BT_TRUE, CYC_PREV_ADDED );
-			cyclingManager.setCyclingStatus( y_min_number,y_min_isBound, CYC_PREV_REMOVED );
-		}
-
-
-		/* 2) Update Lagrange multiplier... */
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] -= y_min * xiC[i];
-		}
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			y[ii] -= y_min * xiB[i];
-		}
-
-		/* ... also for newly active bound ... */
-		if ( B_status == ST_LOWER )
-			y[number] = y_min;
-		else
-			y[number] = -y_min;
-
-		/* ... and for bound to be removed. */
-		if ( y_min_isBound == BT_TRUE )
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"bound no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( removeBound( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[y_min_number] = 0.0;
-		}
-		else
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"constraint no. %d.",y_min_number );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( removeConstraint( y_min_number,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[nV+y_min_number] = 0.0;
-		}
-	}
-	else
-	{
-		/* no constraint/bound can be removed => QP is infeasible! */
-		infeasible = BT_TRUE;
-
-		return THROWERROR( RET_ENSURELI_FAILED_NOINDEX );
-	}
-
-	return getGlobalMessageHandler( )->throwInfo( RET_LI_RESOLVED,0,__FUNCTION__,__FILE__,__LINE__,VS_HIDDEN );
-}
-
-
-
-/*
- *	r e m o v e C o n s t r a i n t
- */
-returnValue QProblem::removeConstraint(	int number,
-										BooleanType updateCholesky
-										)
-{
-	int i, j, ii, jj;
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* some definitions */
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-	int number_idx = constraints.getActive( )->getIndex( number );
-
-
-	/* consistency checks */
-	if ( constraints.getStatus( number ) == ST_INACTIVE )
-		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
-
-	if ( ( number_idx < 0 ) || ( number_idx >= nAC ) )
-		return THROWERROR( RET_CONSTRAINT_NOT_ACTIVE );
-
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
-
-
-	/* I) REMOVE <number>th ROW FROM T,
-	 *    i.e. shift rows number+1 through nAC  upwards (instead of the actual
-	 *    constraint number its corresponding index within matrix A is used). */
-	if ( number_idx < nAC-1 )
-	{
-		for( i=(number_idx+1); i<nAC; ++i )
-			for( j=(nAC-i-1); j<nAC; ++j )
-				T[(i-1)*NVMAX + tcol+j] = T[i*NVMAX + tcol+j];
-		/* gimmick: write zeros into the last row of T */
-		for( j=0; j<nAC; ++j )
-			T[(nAC-1)*NVMAX + tcol+j] = 0.0;
-
-
-		/* II) RESTORE TRIANGULAR FORM OF T,
-		 *     use column-wise Givens rotations to restore reverse triangular form
-		 *     of T simultanenous change of Q (i.e. Y). */
-		real_t c, s;
-
-		for( j=(nAC-2-number_idx); j>=0; --j )
-		{
-			computeGivens( T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j], T[(nAC-2-j)*NVMAX + tcol+1+j],T[(nAC-2-j)*NVMAX + tcol+j],c,s );
-
-			for( i=(nAC-j-1); i<(nAC-1); ++i )
-				applyGivens( c,s,T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j], T[i*NVMAX + tcol+1+j],T[i*NVMAX + tcol+j] );
-
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				applyGivens( c,s,Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j], Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j] );
-			}
-		}
-	}
-	else
-	{
-		/* gimmick: write zeros into the last row of T */
-		for( j=0; j<nAC; ++j )
-			T[(nAC-1)*NVMAX + tcol+j] = 0.0;
-	}
-
-
-	if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-	{
-		/* III) UPDATE CHOLESKY DECOMPOSITION,
-		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
-		 *      of the Cholesky factor R. */
-		real_t Hz[NVMAX];
-		for ( i=0; i<nFR; ++i )
-			Hz[i] = 0.0;
-		real_t rho2 = 0.0;
-
-		/* 1) Calculate Hz = H*z, where z is the new rightmost column of Z
-		 *    (i.e. the old leftmost column of Y).  */
-		for( j=0; j<nFR; ++j )
-		{
-			jj = FR_idx[j];
-			for( i=0; i<nFR; ++i )
-				Hz[i] += H[jj*NVMAX + FR_idx[i]] * Q[jj*NVMAX + nZ];
-		}
-
-		if ( nZ > 0 )
-		{
-			real_t ZHz[NVMAX];
-			for ( i=0; i<nZ; ++i )
-				ZHz[i] = 0.0;
-			real_t r[NVMAX];
-
-			/* 2) Calculate ZHz = Z'*Hz (old Z). */
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-
-				for( i=0; i<nZ; ++i )
-					ZHz[i] += Q[jj*NVMAX + i] * Hz[j];
-			}
-
-			/* 3) Calculate r = R^-T * ZHz. */
-			if ( backsolveR( ZHz,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
-
-			/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
-			 *    and store r into R. */
-			for( i=0; i<nZ; ++i )
-			{
-				rho2 -= r[i]*r[i];
-				R[i*NVMAX + nZ] = r[i];
-			}
-		}
-
-		for( j=0; j<nFR; ++j )
-			rho2 += Q[FR_idx[j]*NVMAX + nZ] * Hz[j];
-
-		/* 5) Store rho into R. */
-		if ( rho2 > 0.0 )
-			R[nZ*NVMAX + nZ] = sqrt( rho2 );
-		else
-		{
-			hessianType = HST_SEMIDEF;
-			return THROWERROR( RET_HESSIAN_NOT_SPD );
-		}
-	}
-
-	/* IV) UPDATE INDICES */
-	if ( constraints.moveActiveToInactive( number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVECONSTRAINT_FAILED );
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e B o u n d
- */
-returnValue QProblem::removeBound(	int number,
-									BooleanType updateCholesky
-									)
-{
-	int i, j, ii, jj;
-
-	/* consistency checks */
-	if ( bounds.getStatus( number ) == ST_INACTIVE )
-		return THROWERROR( RET_BOUND_NOT_ACTIVE );
-
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
- 		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* some definitions */
-	int nFR = getNFR( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	int tcol = sizeT - nAC;
-
-
-	/* I) UPDATE INDICES */
-	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	/* I) APPEND <nFR+1>th UNITY VECOTR TO Q. */
-	int nnFRp1 = FR_idx[nFR];
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		Q[ii*NVMAX + nFR] = 0.0;
-		Q[nnFRp1*NVMAX + i] = 0.0;
-	}
-	Q[nnFRp1*NVMAX + nFR] = 1.0;
-
-	if ( nAC > 0 )
-	{
-		/* store new column a in a temporary vector instead of shifting T one column to the left and appending a */
-		int AC_idx[NCMAX_ALLOC];
-		if ( constraints.getActive( )->getNumberArray( AC_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-		real_t tmp[NCMAX_ALLOC];
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			tmp[i] =  A[ii*NVMAX + number];
-		}
-
-
-		/* II) RESTORE TRIANGULAR FORM OF T,
-		 *     use column-wise Givens rotations to restore reverse triangular form
-		 *     of T = [T A(:,number)], simultanenous change of Q (i.e. Y and Z). */
-		real_t c, s;
-
-		for( j=(nAC-1); j>=0; --j )
-		{
-			computeGivens( tmp[nAC-1-j],T[(nAC-1-j)*NVMAX + tcol+j],T[(nAC-1-j)*NVMAX + tcol+j],tmp[nAC-1-j],c,s );
-
-			for( i=(nAC-j); i<nAC; ++i )
-				applyGivens( c,s,tmp[i],T[i*NVMAX + tcol+j],T[i*NVMAX + tcol+j],tmp[i] );
-
-			for( i=0; i<=nFR; ++i )
-			{
-				ii = FR_idx[i];
-				/* nZ+1+nAC = nFR+1  /  nZ+(1) = nZ+1 */
-				applyGivens( c,s,Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j],Q[ii*NVMAX + nZ+1+j],Q[ii*NVMAX + nZ+j] );
-			}
-		}
-	}
-
-
-	if ( ( updateCholesky == BT_TRUE ) && ( hessianType != HST_IDENTITY ) )
-	{
-		/* III) UPDATE CHOLESKY DECOMPOSITION,
-		 *      calculate new additional column (i.e. [r sqrt(rho2)]')
-		 *      of the Cholesky factor R: */
-		real_t z2 = Q[nnFRp1*NVMAX + nZ];
-		real_t rho2 = H[nnFRp1*NVMAX + nnFRp1]*z2*z2; /* rho2 = h2*z2*z2 */
-
-		if ( nFR > 0 )
-		{
-			real_t Hz[NVMAX];
-			for( i=0; i<nFR; ++i )
-				Hz[i] = 0.0;
-			/* 1) Calculate R'*r = Zfr'*Hfr*z1 + z2*Zfr'*h1 =: Zfr'*Hz + z2*Zfr'*h1 =: rhs and
-			 *    rho2 = z1'*Hfr*z1 + 2*z2*h1'*z1 + h2*z2^2 - r'*r =: z1'*Hz + 2*z2*h1'*z1 + h2*z2^2 - r'r */
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					/*			   H * z1 */
-					Hz[i] += H[jj*NVMAX + ii] * Q[jj*NVMAX + nZ];
-				}
-			}
-
-			if ( nZ > 0 )
-			{
-				real_t r[NVMAX];
-				real_t rhs[NVMAX];
-				for( i=0; i<nZ; ++i )
-					rhs[i] = 0.0;
-
-				/* 2) Calculate rhs. */
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					for( i=0; i<nZ; ++i )
-										/* Zfr' * ( Hz + z2*h1 ) */
-						rhs[i] += Q[jj*NVMAX + i] * ( Hz[j] + z2 * H[nnFRp1*NVMAX + jj] );
-				}
-
-				/* 3) Calculate r = R^-T * rhs. */
-				if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-				/* 4) Calculate rho2 = rho^2 = z'*Hz - r'*r
-				 *    and store r into R. */
-				for( i=0; i<nZ; ++i )
-				{
-					rho2 -= r[i]*r[i];
-					R[i*NVMAX + nZ] = r[i];
-				}
-			}
-
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-							/* z1' * ( Hz + 2*z2*h1 ) */
-				rho2 += Q[jj*NVMAX + nZ] * ( Hz[j] + 2.0*z2*H[nnFRp1*NVMAX + jj] );
-			}
-		}
-
-
-		/* 5) Store rho into R. */
-		if ( rho2 > 0.0 )
-			R[nZ*NVMAX + nZ] = sqrt( rho2 );
-		else
-		{
-			hessianType = HST_SEMIDEF;
-			return THROWERROR( RET_HESSIAN_NOT_SPD );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	b a c k s o l v e R  (CODE DUPLICATE OF QProblemB CLASS!!!)
- */
-returnValue QProblem::backsolveR(	const real_t* const b, BooleanType transposed,
-									real_t* const a
-									)
-{
-	/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
-	return backsolveR( b,transposed,BT_FALSE,a );
-}
-
-
-/*
- *	b a c k s o l v e R  (CODE DUPLICATE OF QProblemB CLASS!!!)
- */
-returnValue QProblem::backsolveR(	const real_t* const b, BooleanType transposed,
-									BooleanType removingBound,
-									real_t* const a
-									)
-{
-	int i, j;
-	int nR = getNZ( );
-
-	real_t sum;
-
-	/* if backsolve is called while removing a bound, reduce nZ by one. */
-	if ( removingBound == BT_TRUE )
-		--nR;
-
-	/* nothing to do */
-	if ( nR <= 0 )
-		return SUCCESSFUL_RETURN;
-
-
-	/* Solve Ra = b, where R might be transposed. */
-	if ( transposed == BT_FALSE )
-	{
-		/* solve Ra = b */
-		for( i=(nR-1); i>=0; --i )
-		{
-			sum = b[i];
-			for( j=(i+1); j<nR; ++j )
-				sum -= R[i*NVMAX + j] * a[j];
-
-			if ( getAbs( R[i*NVMAX + i] ) > ZERO )
-				a[i] = sum / R[i*NVMAX + i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-	else
-	{
-		/* solve R^T*a = b */
-		for( i=0; i<nR; ++i )
-		{
-			sum = b[i];
-
-			for( j=0; j<i; ++j )
-				sum -= R[j*NVMAX + i] * a[j];
-
-			if ( getAbs( R[i*NVMAX + i] ) > ZERO )
-				a[i] = sum / R[i*NVMAX + i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	b a c k s o l v e T
- */
-returnValue QProblem::backsolveT( const real_t* const b, BooleanType transposed, real_t* const a )
-{
-	int i, j;
-	int nT = getNAC( );
-	int tcol = sizeT - nT;
-
-	real_t sum;
-
-	/* nothing to do */
-	if ( nT <= 0 )
-		return SUCCESSFUL_RETURN;
-
-
-	/* Solve Ta = b, where T might be transposed. */
-	if ( transposed == BT_FALSE )
-	{
-		/* solve Ta = b */
-		for( i=0; i<nT; ++i )
-		{
-			sum = b[i];
-			for( j=0; j<i; ++j )
-				sum -= T[i*NVMAX + sizeT-1-j] * a[nT-1-j];
-
-			if ( getAbs( T[i*NVMAX + sizeT-1-i] ) > ZERO )
-				a[nT-1-i] = sum / T[i*NVMAX + sizeT-1-i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-	else
-	{
-		/* solve T^T*a = b */
-		for( i=0; i<nT; ++i )
-		{
-			sum = b[i];
-			for( j=0; j<i; ++j )
-				sum -= T[(nT-1-j)*NVMAX + tcol+i] * a[nT-1-j];
-
-			if ( getAbs( T[(nT-1-i)*NVMAX + tcol+i] ) > ZERO )
-				a[nT-1-i] = sum / T[(nT-1-i)*NVMAX + tcol+i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ d e t e r m i n e D a t a S h i f t
- */
-returnValue QProblem::hotstart_determineDataShift(  const int* const FX_idx, const int* const AC_idx,
-													const real_t* const g_new, const real_t* const lbA_new, const real_t* const ubA_new,
-													const real_t* const lb_new, const real_t* const ub_new,
-													real_t* const delta_g, real_t* const delta_lbA, real_t* const delta_ubA,
-													real_t* const delta_lb, real_t* const delta_ub,
-													BooleanType& Delta_bC_isZero, BooleanType& Delta_bB_isZero
-													)
-{
-	int i, ii;
-	int nC  = getNC( );
-	int nAC = getNAC( );
-
-
-	/* I) DETERMINE DATA SHIFT FOR BOUNDS */
-	QProblemB::hotstart_determineDataShift( FX_idx,g_new,lb_new,ub_new, delta_g,delta_lb,delta_ub, Delta_bB_isZero );
-
-
-	/* II) DETERMINE DATA SHIFT FOR CONSTRAINTS */
-	/* 1) Calculate shift directions. */
-	for( i=0; i<nC; ++i )
-	{
-		/* if lower constraints' bounds do not exist, shift them to -infinity */
-		if ( lbA_new != 0 )
-			delta_lbA[i] = lbA_new[i] - lbA[i];
-		else
-			delta_lbA[i] = -INFTY - lbA[i];
-	}
-
-	for( i=0; i<nC; ++i )
-	{
-		/* if upper constraints' bounds do not exist, shift them to infinity */
-		if ( ubA_new != 0 )
-			delta_ubA[i] = ubA_new[i] - ubA[i];
-		else
-			delta_ubA[i] = INFTY - ubA[i];
-	}
-
-	/* 2) Determine if active constraints' bounds are to be shifted. */
-	Delta_bC_isZero = BT_TRUE;
-
-	for ( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		if ( ( getAbs( delta_lbA[ii] ) > EPS ) || ( getAbs( delta_ubA[ii] ) > EPS ) )
-		{
-			Delta_bC_isZero = BT_FALSE;
-			break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n
- */
-returnValue QProblem::hotstart_determineStepDirection(	const int* const FR_idx, const int* const FX_idx, const int* const AC_idx,
-														const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
-														const real_t* const delta_lb, const real_t* const delta_ub,
-														BooleanType Delta_bC_isZero, BooleanType Delta_bB_isZero,
-														real_t* const delta_xFX, real_t* const delta_xFR,
-														real_t* const delta_yAC, real_t* const delta_yFX
-														)
-{
-	int i, j, ii, jj;
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nZ  = getNZ( );
-
-	/* initialise auxiliary vectors */
-	real_t HMX_delta_xFX[NVMAX];
-	real_t YFR_delta_xFRy[NVMAX];
-	real_t ZFR_delta_xFRz[NVMAX];
-	real_t HFR_YFR_delta_xFRy[NVMAX];
-	for( i=0; i<nFR; ++i )
-	{
-		delta_xFR[i] = 0.0;
-		HMX_delta_xFX[i] = 0.0;
-		YFR_delta_xFRy[i] = 0.0;
-		ZFR_delta_xFRz[i] = 0.0;
-		HFR_YFR_delta_xFRy[i] = 0.0;
-	}
-
-	real_t delta_xFRy[NCMAX_ALLOC];
-	real_t delta_xFRz[NVMAX];
-	for( i=0; i<nZ; ++i )
-		delta_xFRz[i] = 0.0;
-
-
-	/* I) DETERMINE delta_xFX */
-	if ( nFX > 0 )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-				delta_xFX[i] = delta_lb[ii];
-			else
-				delta_xFX[i] = delta_ub[ii];
-		}
-	}
-
-	/* II) DETERMINE delta_xFR */
-	if ( nFR > 0 )
-	{
-		/* 1) Determine delta_xFRy. */
-		if ( nAC > 0 )
-		{
-			if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
-			{
-				for( i=0; i<nAC; ++i )
-					delta_xFRy[i] = 0.0;
-
-				for( i=0; i<nFR; ++i )
-					delta_xFR[i] = 0.0;
-			}
-			else
-			{
-				/* auxillary variable */
-				real_t delta_xFRy_TMP[NCMAX_ALLOC];
-
-				for( i=0; i<nAC; ++i )
-				{
-					ii = AC_idx[i];
-
-					if ( constraints.getStatus( ii ) == ST_LOWER )
-						delta_xFRy_TMP[i] = delta_lbA[ii];
-					else
-						delta_xFRy_TMP[i] = delta_ubA[ii];
-
-					if ( Delta_bB_isZero == BT_FALSE )
-					{
-						for( j=0; j<nFX; ++j )
-						{
-							jj = FX_idx[j];
-							delta_xFRy_TMP[i] -= A[ii*NVMAX + jj] * delta_xFX[j];
-						}
-					}
-				}
-
-				if ( backsolveT( delta_xFRy_TMP, BT_FALSE, delta_xFRy ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
-
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					for( j=0; j<nAC; ++j )
-						YFR_delta_xFRy[i] += Q[ii*NVMAX + nZ+j] * delta_xFRy[j];
-
-					/* delta_xFR = YFR*delta_xFRy (+ ZFR*delta_xFRz) */
-					delta_xFR[i] = YFR_delta_xFRy[i];
-				}
-			}
-		}
-
-		/* 2) Determine delta_xFRz. */
-		if ( hessianType == HST_IDENTITY )
-		{
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				for( i=0; i<nZ; ++i )
-					delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_g[jj];
-			}
-
-			if ( nZ > 0 )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					for( j=0; j<nZ; ++j )
-						ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];
-
-					delta_xFR[i] += ZFR_delta_xFRz[i];
-				}
-			}
-		}
-		else
-		{
-			if ( Delta_bB_isZero == BT_FALSE )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					for( j=0; j<nFX; ++j )
-					{
-						jj = FX_idx[j];
-						HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
-					}
-				}
-			}
-
-			if ( nAC > 0 )
-			{
-				if ( ( Delta_bC_isZero == BT_FALSE ) || ( Delta_bB_isZero == BT_FALSE ) )
-				{
-					for( i=0; i<nFR; ++i )
-					{
-						ii = FR_idx[i];
-						for( j=0; j<nFR; ++j )
-						{
-							jj = FR_idx[j];
-							HFR_YFR_delta_xFRy[i] += H[ii*NVMAX + jj] * YFR_delta_xFRy[j];
-						}
-					}
-				}
-			}
-
-
-			if ( nZ > 0 )
-			{
-				/* auxiliary variable */
-				real_t delta_xFRz_TMP[NVMAX];
-				real_t delta_xFRz_RHS[NVMAX];
-
-
-				if ( ( nAC > 0 ) && ( nFX > 0 ) && ( Delta_bB_isZero == BT_FALSE ) )
-				{
-					for( j=0; j<nFR; ++j )
-					{
-						jj = FR_idx[j];
-						delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j] + HMX_delta_xFX[j];
-					}
-				}
-				else
-				{
-					if ( ( nAC == 0 ) && ( Delta_bB_isZero == BT_TRUE ) )
-					{
-						for( j=0; j<nFR; ++j )
-						{
-							jj = FR_idx[j];
-							delta_xFRz_RHS[j] = delta_g[jj];
-						}
-					}
-					else
-					{
-						if ( nAC > 0 ) /* => Delta_bB_isZero == BT_TRUE, as BT_FALSE would imply nFX>0 */
-						{
-							for( j=0; j<nFR; ++j )
-							{
-								jj = FR_idx[j];
-								delta_xFRz_RHS[j] = delta_g[jj] + HFR_YFR_delta_xFRy[j];
-							}
-						}
-						else /* Delta_bB_isZero == BT_FALSE, as nAC==0 */
-						{
-							for( j=0; j<nFR; ++j )
-							{
-								jj = FR_idx[j];
-								delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j];
-							}
-						}
-					}
-				}
-
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					for( i=0; i<nZ; ++i )
-						delta_xFRz[i] -= Q[jj*NVMAX + i] * delta_xFRz_RHS[j];
-				}
-
-
-				if ( backsolveR( delta_xFRz,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-
-				if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFRz ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-
-
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					for( j=0; j<nZ; ++j )
-						ZFR_delta_xFRz[i] += Q[ii*NVMAX + j] * delta_xFRz[j];
-
-					delta_xFR[i] += ZFR_delta_xFRz[i];
-				}
-			}
-		}
-	}
-
-	/* III) DETERMINE delta_yAC */
-	if ( nAC > 0 ) /* => ( nFR = nZ + nAC > 0 ) */
-	{
-		/* auxiliary variables */
-		real_t delta_yAC_TMP[NCMAX_ALLOC];
-		for( i=0; i<nAC; ++i )
-			delta_yAC_TMP[i] = 0.0;
-		real_t delta_yAC_RHS[NVMAX];
-		for( i=0; i<nFR; ++i )
-			delta_yAC_RHS[i] = 0.0;
-
-		if ( hessianType == HST_IDENTITY )
-		{
-			/* delta_yAC = (T')^-1 * ( Yfr*delta_gFR + delta_xFRy ) */
-			for( j=0; j<nAC; ++j )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					delta_yAC_TMP[j] += Q[ii*NVMAX + nZ+j] * delta_g[ii];
-				}
-
-				delta_yAC_TMP[j] += delta_xFRy[j];
-			}
-		}
-		else
-		{
-			if ( ( Delta_bC_isZero == BT_TRUE ) && ( Delta_bB_isZero == BT_TRUE ) )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					delta_yAC_RHS[i] = delta_g[ii];
-				}
-			}
-			else
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					delta_yAC_RHS[i] = HFR_YFR_delta_xFRy[i] + delta_g[ii];
-				}
-			}
-
-			if ( nZ > 0 )
-			{
-				for( i=0; i<nFR; ++i )
-				{
-					ii = FR_idx[i];
-					for( j=0; j<nFR; ++j )
-					{
-						jj = FR_idx[j];
-						delta_yAC_RHS[i] += H[ii*NVMAX + jj] * ZFR_delta_xFRz[j];
-					}
-				}
-			}
-
-			if ( nFX > 0 )
-			{
-				if ( Delta_bB_isZero == BT_FALSE )
-				{
-					for( i=0; i<nFR; ++i )
-						delta_yAC_RHS[i] += HMX_delta_xFX[i];
-				}
-			}
-
-			for( i=0; i<nAC; ++i)
-			{
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					delta_yAC_TMP[i] += Q[jj*NVMAX + nZ+i] * delta_yAC_RHS[j];
-				}
-			}
-		}
-
-		if ( backsolveT( delta_yAC_TMP,BT_TRUE,delta_yAC ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_STEPDIRECTION_FAILED_TQ );
-	}
-
-
-	/* IV) DETERMINE delta_yFX */
-	if ( nFX > 0 )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			delta_yFX[i] = delta_g[ii];
-
-			for( j=0; j<nAC; ++j )
-			{
-				jj = AC_idx[j];
-				delta_yFX[i] -= A[jj*NVMAX + ii] * delta_yAC[j];
-			}
-
-			if ( hessianType == HST_IDENTITY )
-			{
-				delta_yFX[i] += delta_xFX[i];
-			}
-			else
-			{
-				for( j=0; j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
-				}
-
-				if ( Delta_bB_isZero == BT_FALSE )
-				{
-					for( j=0; j<nFX; ++j )
-					{
-						jj = FX_idx[j];
-						delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
-					}
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ d e t e r m i n e S t e p L e n g t h
- */
-returnValue QProblem::hotstart_determineStepLength(	const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
-													const real_t* const delta_lbA, const real_t* const delta_ubA,
-													const real_t* const delta_lb, const real_t* const delta_ub,
-													const real_t* const delta_xFX, const real_t* const delta_xFR,
-													const real_t* const delta_yAC, const real_t* const delta_yFX,
-													real_t* const delta_Ax, int& BC_idx, SubjectToStatus& BC_status, BooleanType& BC_isBound
-													)
-{
-	int i, j, ii, jj;
-	int nV  = getNV( );
-	int nC  = getNC( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nIAC = getNIAC( );
-
-	/* initialise maximum steplength array */
-	real_t maxStepLength[2*(NVMAX+NCMAX_ALLOC)];
-	for ( i=0; i<2*(nV+nC); ++i )
-		maxStepLength[i] = 1.0;
-
-
-	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH: */
-	/* 1) Ensure that active dual constraints' bounds remain valid
-	 *    (ignoring inequality constraints).  */
-	for( i=0; i<nAC; ++i )
-	{
-		ii = AC_idx[i];
-
-		if ( constraints.getType( ii ) != ST_EQUALITY )
-		{
-			if ( constraints.getStatus( ii ) == ST_LOWER )
-			{
-				/* active lower constraints' bounds */
-				if ( delta_yAC[i] < -ZERO )
-				{
-					if ( y[nV+ii] > 0.0 )
-						maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
-					else
-						maxStepLength[nV+ii] = 0.0;
-				}
-			}
-			else
-			{
-				/* active upper constraints' bounds */
-				if ( delta_yAC[i] > ZERO )
-				{
-					if ( y[nV+ii] < 0.0 )
-						maxStepLength[nV+ii] = y[nV+ii] / ( -delta_yAC[i] );
-					else
-						maxStepLength[nV+ii] = 0.0;
-				}
-			}
-		}
-	}
-
-	/* 2) Ensure that active dual bounds remain valid
-	 *    (ignoring implicitly fixed variables). */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( bounds.getType( ii ) != ST_EQUALITY )
-		{
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-			{
-				/* active lower bounds */
-				if ( delta_yFX[i] < -ZERO )
-				{
-					if ( y[ii] > 0.0 )
-						maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
-					else
-						maxStepLength[ii] = 0.0;
-				}
-			}
-			else
-			{
-				/* active upper bounds */
-				if ( delta_yFX[i] > ZERO )
-				{
-					if ( y[ii] < 0.0 )
-						maxStepLength[ii] = y[ii] / ( -delta_yFX[i] );
-					else
-						maxStepLength[ii] = 0.0;
-				}
-			}
-		}
-	}
-
-
-	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH */
-	/* 1) Ensure that inactive constraints' bounds remain valid
-	 *    (ignoring unbounded constraints). */
-	real_t delta_x[NVMAX];
-	for( j=0; j<nFR; ++j )
-	{
-		jj = FR_idx[j];
-		delta_x[jj] = delta_xFR[j];
-	}
-	for( j=0; j<nFX; ++j )
-	{
-		jj = FX_idx[j];
-		delta_x[jj] = delta_xFX[j];
-	}
-
-	for( i=0; i<nIAC; ++i )
-	{
-		ii = IAC_idx[i];
-
-		if ( constraints.getType( ii ) != ST_UNBOUNDED )
-		{
-			delta_Ax[ii] = 0.0;
-			for( j=0; j<nV; ++j )
-				delta_Ax[ii] += A[ii*NVMAX + j] * delta_x[j]; // POSSIBLE SPEEDUP!
-
-			/* inactive lower constraints' bounds */
-			if ( constraints.isNoLower( ) == BT_FALSE )
-			{
-				if ( delta_lbA[ii] > delta_Ax[ii] )
-				{
-					if ( Ax[ii] > lbA[ii] )
-						maxStepLength[nV+ii] = ( Ax[ii] - lbA[ii] ) / ( delta_lbA[ii] - delta_Ax[ii] );
-					else
-						maxStepLength[nV+ii] = 0.0;
-				}
-			}
-
-			/* inactive upper constraints' bounds */
-			if ( constraints.isNoUpper( ) == BT_FALSE )
-			{
-				if ( delta_ubA[ii] < delta_Ax[ii] )
-				{
-					if ( Ax[ii] < ubA[ii] )
-						maxStepLength[nV+nC+nV+ii] = ( Ax[ii] - ubA[ii] ) / ( delta_ubA[ii] - delta_Ax[ii] );
-					else
-						maxStepLength[nV+nC+nV+ii] = 0.0;
-				}
-			}
-		}
-	}
-
-
-	/* 2) Ensure that inactive bounds remain valid
-	 *    (ignoring unbounded variables). */
-	/* inactive lower bounds */
-	if ( bounds.isNoLower( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			if ( bounds.getType( ii ) != ST_UNBOUNDED )
-				if ( delta_lb[ii] > delta_xFR[i] )
-				{
-					if ( x[ii] > lb[ii] )
-						maxStepLength[ii] = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
-					else
-						maxStepLength[ii] = 0.0;
-				}
-		}
-	}
-
-	/* inactive upper bounds */
-	if ( bounds.isNoUpper( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			if ( bounds.getType( ii ) != ST_UNBOUNDED )
-				if ( delta_ub[ii] < delta_xFR[i] )
-				{
-					if ( x[ii] < ub[ii] )
-						maxStepLength[nV+nC+ii] = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
-					else
-						maxStepLength[nV+nC+ii] = 0.0;
-				}
-		}
-	}
-
-
-	/* III) DETERMINE MAXIMUM HOMOTOPY STEPLENGTH */
-	real_t tau_new = 1.0;
-
-	BC_idx = 0;
-	BC_status = ST_UNDEFINED;
-	BC_isBound = BT_FALSE;
-
-	for ( i=0; i<nV; ++i )
-	{
-		/* 1) Consider lower/dual blocking bounds. */
-		if ( maxStepLength[i] < tau_new )
-		{
-			tau_new = maxStepLength[i];
-			BC_idx = i;
-			BC_isBound = BT_TRUE;
-			if ( bounds.getStatus( i ) == ST_INACTIVE ) /* inactive? */
-				BC_status = ST_LOWER;
-			else
-				BC_status = ST_INACTIVE;
-		}
-
-		/* 2) Consider upper blocking bounds. */
-		if ( maxStepLength[nV+nC+i] < tau_new )
-		{
-			tau_new = maxStepLength[nV+nC+i];
-			BC_idx = i;
-			BC_isBound = BT_TRUE;
-			BC_status = ST_UPPER;
-		}
-	}
-
-	for ( i=nV; i<nV+nC; ++i )
-	{
-		/* 3) Consider lower/dual blocking constraints. */
-		if ( maxStepLength[i] < tau_new )
-		{
-			tau_new = maxStepLength[i];
-			BC_idx = i-nV;
-			BC_isBound = BT_FALSE;
-			if ( constraints.getStatus( i-nV ) == ST_INACTIVE ) /* inactive? */
-				BC_status = ST_LOWER;
-			else
-				BC_status = ST_INACTIVE;
-		}
-
-		/* 4) Consider upper blocking constraints. */
-		if ( maxStepLength[nV+nC+i] < tau_new )
-		{
-			tau_new = maxStepLength[nV+nC+i];
-			BC_idx = i-nV;
-			BC_isBound = BT_FALSE;
-			BC_status = ST_UPPER;
-		}
-	}
-
-
-	/* IV) CLEAR CYCLING DATA
-	 *     if a positive step can be taken */
-	if ( tau_new > EPS )
-		cyclingManager.clearCyclingData( );
-
-
-	/* V) SET MAXIMUM HOMOTOPY STEPLENGTH */
-	tau = tau_new;
-
-	#ifdef PC_DEBUG
-	if ( printlevel == PL_HIGH )
-	{
-
-	 	char messageString[80];
-
-		if ( BC_status == ST_UNDEFINED )
-			sprintf( messageString,"Stepsize is %.6e!",tau );
-		else
-			sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_isBound = %d, BC_status = %d)",tau,BC_idx,BC_isBound,BC_status );
-
-		getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	}
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ p e r f o r m S t e p
- */
-returnValue QProblem::hotstart_performStep(	const int* const FR_idx, const int* const FX_idx, const int* const AC_idx, const int* const IAC_idx,
-											const real_t* const delta_g, const real_t* const delta_lbA, const real_t* const delta_ubA,
-											const real_t* const delta_lb, const real_t* const delta_ub,
-											const real_t* const delta_xFX, const real_t* const delta_xFR,
-											const real_t* const delta_yAC, const real_t* const delta_yFX,
-											const real_t* const delta_Ax, int BC_idx, SubjectToStatus BC_status, BooleanType BC_isBound
-											)
-{
-	int i, j, ii;
-	int nV  = getNV( );
-	int nC  = getNC( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-	int nAC = getNAC( );
-	int nIAC = getNIAC( );
-
-
-	/* I) CHECK (CONSTRAINTS') BOUNDS' CONSISTENCY */
-	if ( areBoundsConsistent( delta_lb,delta_ub,delta_lbA,delta_ubA ) == BT_FALSE )
-	{
-		infeasible = BT_TRUE;
-		tau = 0.0;
-
-		return THROWERROR( RET_QP_INFEASIBLE );
-	}
-
-
-	/* II) GO TO ACTIVE SET CHANGE */
-	if ( tau > ZERO )
-	{
-		/* 1) Perform step in primal und dual space... */
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			x[ii] += tau*delta_xFR[i];
-		}
-
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			x[ii] += tau*delta_xFX[i];
-			y[ii] += tau*delta_yFX[i];
-		}
-
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-			y[nV+ii] += tau*delta_yAC[i];
-		}
-
-		/* ... also for Ax. */
-		for( i=0; i<nIAC; ++i )
-		{
-			ii = IAC_idx[i];
-			if ( constraints.getType( ii ) != ST_UNBOUNDED )
-				Ax[ii] += tau*delta_Ax[ii];
-		}
-		for( i=0; i<nAC; ++i )
-		{
-			ii = AC_idx[i];
-
-			Ax[ii] = 0.0;
-			for( j=0; j<nV; ++j )
-				Ax[ii] += A[ii*NVMAX + j] * x[j];
-		}
-
-		/* 2) Shift QP data. */
-		for( i=0; i<nV; ++i )
-		{
-			g[i]  += tau*delta_g[i];
-			lb[i] += tau*delta_lb[i];
-			ub[i] += tau*delta_ub[i];
-		}
-
-		for( i=0; i<nC; ++i )
-		{
-			lbA[i] += tau*delta_lbA[i];
-			ubA[i] += tau*delta_ubA[i];
-		}
-	}
-	else
-	{
-		/* print a stepsize warning if stepsize is zero */
-		#ifdef PC_DEBUG
-		char messageString[80];
-		sprintf( messageString,"Stepsize is %.6e",tau );
-		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-
-	/* setup output preferences */
-	#ifdef PC_DEBUG
-	char messageString[80];
-	VisibilityStatus visibilityStatus;
-
-	if ( printlevel == PL_HIGH )
-		visibilityStatus = VS_VISIBLE;
-	else
-		visibilityStatus = VS_HIDDEN;
-	#endif
-
-	
-	/* III) UPDATE ACTIVE SET */
-	switch ( BC_status )
-	{
-		/* Optimal solution found as no working set change detected. */
-		case ST_UNDEFINED:
-			return RET_OPTIMAL_SOLUTION_FOUND;
-
-
-		/* Remove one variable from active set. */
-		case ST_INACTIVE:
-			if ( BC_isBound == BT_TRUE )
-			{
-				#ifdef PC_DEBUG
-				sprintf( messageString,"bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-				#endif
-
-				if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-				y[BC_idx] = 0.0;
-			}
-			else
-			{
-				#ifdef PC_DEBUG
-				sprintf( messageString,"constraint no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-				#endif
-
-				if ( removeConstraint( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-				y[nV+BC_idx] = 0.0;
-			}
-			break;
-
-
-		/* Add one variable to active set. */
-		default:
-			if ( BC_isBound == BT_TRUE )
-			{
-				#ifdef PC_DEBUG
-				if ( BC_status == ST_LOWER )
-					sprintf( messageString,"lower bound no. %d.", BC_idx );
-				else
-					sprintf( messageString,"upper bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-				#endif
-
-				if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			}
-			else
-			{
-				#ifdef PC_DEBUG
-				if ( BC_status == ST_LOWER )
-					sprintf( messageString,"lower constraint's bound no. %d.", BC_idx );
-				else
-					sprintf( messageString,"upper constraint's bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-				#endif
-
-				if ( addConstraint( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			}
-			break;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a r e B o u n d s C o n s i s t e n t
- */
-BooleanType QProblem::areBoundsConsistent(	const real_t* const delta_lb, const real_t* const delta_ub,
-											const real_t* const delta_lbA, const real_t* const delta_ubA
-											) const
-{
-	int i;
-
-	/* 1) Check bounds' consistency. */
-	if ( QProblemB::areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
-		return BT_FALSE;
-
-	/* 2) Check constraints' consistency, i.e.
-	 *    check if delta_lb[i] is greater than delta_ub[i]
-	 *    for a component i whose bounds are already (numerically) equal. */
-	for( i=0; i<getNC( ); ++i )
-		if ( ( lbA[i] > ubA[i] - BOUNDTOL ) && ( delta_lbA[i] > delta_ubA[i] + EPS ) )
-			return BT_FALSE;
-
-	return BT_TRUE;
-}
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblem::setupQPdata(	const real_t* const _H, const real_t* const _R, const real_t* const _g, const real_t* const _A,
-									const real_t* const _lb, const real_t* const _ub,
-									const real_t* const _lbA, const real_t* const _ubA
-									)
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-
-	/* 1) Load Hessian matrix as well as lower and upper bounds vectors. */
-	if (QProblemB::setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Load constraint matrix. */
-	if ( ( nC > 0 ) && ( _A == 0 ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	if ( nC > 0 )
-	{
-		for( i=0; i<nC; ++i )
-			for( j=0; j<nV; ++j )
-				A[i*NVMAX + j] = _A[i*nV + j];
-
-		/* 3) Setup lower constraints' bounds vector. */
-		if ( _lbA != 0 )
-		{
-			for( i=0; i<nC; ++i )
-				lbA[i] = _lbA[i];
-		}
-		else
-		{
-			/* if no lower constraints' bounds are specified, set them to -infinity */
-			for( i=0; i<nC; ++i )
-				lbA[i] = -INFTY;
-		}
-
-		/* 4) Setup upper constraints' bounds vector. */
-		if ( _ubA != 0 )
-		{
-			for( i=0; i<nC; ++i )
-				ubA[i] = _ubA[i];
-		}
-		else
-		{
-			/* if no upper constraints' bounds are specified, set them to infinity */
-			for( i=0; i<nC; ++i )
-				ubA[i] = INFTY;
-		}
-	}
-
-// 	printmatrix2( "A",A,10,20 );
-	
-// 	printmatrix2( "lbA",lbA,1,nC );
-// 	printmatrix2( "ubA",ubA,1,nC );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-
-/*
- *	p r i n t I t e r a t i o n
- */
-returnValue QProblem::printIteration( 	int iteration,
-										int BC_idx,	SubjectToStatus BC_status, BooleanType BC_isBound
-		  								)
-{
-	char myPrintfString[160];
-
-	/* consistency check */
-	if ( iteration < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* nothing to do */
-	if ( printlevel != PL_MEDIUM )
-		return SUCCESSFUL_RETURN;
-
-
-	/* 1) Print header at first iteration. */
- 	if ( iteration == 0 )
-	{
-		sprintf( myPrintfString,"\n##############  qpOASES  --  QP NO.%4.1d  ###############\n", count );
-		myPrintf( myPrintfString );
-
-		sprintf( myPrintfString,"  Iter  |  StepLength   |     Info      |  nFX  |  nAC  \n" );
-		myPrintf( myPrintfString );
-	}
-
-	/* 2) Print iteration line. */
-	if ( BC_status == ST_UNDEFINED )
-	{
-		sprintf( myPrintfString,"  %4.1d  |  %1.5e  |   QP SOLVED   | %4.1d  | %4.1d  \n", iteration,tau,getNFX( ),getNAC( ) );
-		myPrintf( myPrintfString );
-	}
-	else
-	{
-		char info[8];
-
-		if ( BC_status == ST_INACTIVE )
-			sprintf( info,"REM " );
-		else
-			sprintf( info,"ADD " );
-
-		if ( BC_isBound == BT_TRUE )
-			sprintf( &(info[4]),"BND" );
-		else
-			sprintf( &(info[4]),"CON" );
-
-		sprintf( myPrintfString,"  %4.1d  |  %1.5e  |  %s%4.1d  | %4.1d  | %4.1d  \n", iteration,tau,info,BC_idx,getNFX( ),getNAC( ) );
-		myPrintf( myPrintfString );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t I t e r a t i o n
- */
-returnValue QProblem::printIteration( 	int iteration,
-										int BC_idx,	SubjectToStatus BC_status
-		  								)
-{
-	return printIteration( iteration,BC_idx,BC_status,BT_TRUE );
-}
-
-#endif  /* PC_DEBUG */
-
-
-
-/*
- *	c h e c k K K T c o n d i t i o n s
- */
-returnValue QProblem::checkKKTconditions( )
-{
-	#ifdef __PERFORM_KKT_TEST__
-
-	int i, j, jj;
-	int nV  = getNV( );
-	int nC  = getNC( );
-	int nAC = getNAC( );
-
-	real_t tmp;
-	real_t maxKKTviolation = 0.0;
-
-	int AC_idx[NCMAX_ALLOC];
-	constraints.getActive( )->getNumberArray( AC_idx );
-
-	/* 1) check for Hx + g - [yFX yAC]*[Id A]' = 0. */
-	for( i=0; i<nV; ++i )
-	{
-		tmp = g[i];
-
-		for( j=0; j<nV; ++j )
-			tmp += H[i*NVMAX + j] * x[j];
-
-		tmp -= y[i];
-
-		/* Only sum over active constraints as y is zero for all inactive ones. */
-		for( j=0; j<nAC; ++j )
-		{
-			jj = AC_idx[j];
-			tmp -= A[jj*NVMAX + i] * y[nV+jj];
-		}
-
-		if ( getAbs( tmp ) > maxKKTviolation )
-			maxKKTviolation = getAbs( tmp );
-	}
-
-	/* 2) Check for [lb lbA] <= [Id A]*x <= [ub ubA]. */
-	/* lbA <= Ax <= ubA */
-	for( i=0; i<nC; ++i )
-	{
-		if ( lbA[i] - Ax[i] > maxKKTviolation )
-			maxKKTviolation = lbA[i] - Ax[i];
-
-		if ( Ax[i] - ubA[i] > maxKKTviolation )
-			maxKKTviolation = Ax[i] - ubA[i];
-	}
-
-	/* lb <= x <= ub */
-	for( i=0; i<nV; ++i )
-	{
-		if ( lb[i] - x[i] > maxKKTviolation )
-			maxKKTviolation = lb[i] - x[i];
-
-		if ( x[i] - ub[i] > maxKKTviolation )
-			maxKKTviolation = x[i] - ub[i];
-	}
-
-	/* 3) Check for correct sign of y and for complementary slackness. */
-	/* bounds */
-	for( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -y[i] > maxKKTviolation )
-					maxKKTviolation = -y[i];
-				if ( getAbs( x[i] - lb[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( x[i] - lb[i] );
-				break;
-
-			case ST_UPPER:
-				if ( y[i] > maxKKTviolation )
-					maxKKTviolation = y[i];
-				if ( getAbs( ub[i] - x[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ub[i] - x[i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( y[i] );
-				break;
-		}
-	}
-
-	/* constraints */
-	for( i=0; i<nC; ++i )
-	{
-		switch ( constraints.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -y[nV+i] > maxKKTviolation )
-					maxKKTviolation = -y[nV+i];
-				if ( getAbs( Ax[i] - lbA[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( Ax[i] - lbA[i] );
-				break;
-
-			case ST_UPPER:
-				if ( y[nV+i] > maxKKTviolation )
-					maxKKTviolation = y[nV+i];
-				if ( getAbs( ubA[i] - Ax[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ubA[i] - Ax[i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( y[nV+i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( y[nV+i] );
-				break;
-		}
-	}
-
-	if ( maxKKTviolation > CRITICALACCURACY )
-		return RET_NO_SOLUTION;
-
-	if ( maxKKTviolation > DESIREDACCURACY )
-		return RET_INACCURATE_SOLUTION;
-
-	#endif /* __PERFORM_KKT_TEST__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/QProblem.ipp b/phonelibs/qpoases/SRC/QProblem.ipp
deleted file mode 100644
index e98ada633..000000000
--- a/phonelibs/qpoases/SRC/QProblem.ipp
+++ /dev/null
@@ -1,299 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/QProblem.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the QProblem class which 
- *	is able to use the newly developed online active set strategy for 
- *	parametric quadratic programming.
- */
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t A
- */
-inline returnValue QProblem::getA( real_t* const _A ) const
-{
-	int i;
-
-	for ( i=0; i<getNV( )*getNC( ); ++i )
-		_A[i] = A[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t A
- */
-inline returnValue QProblem::getA( int number, real_t* const row ) const
-{
-	int nV = getNV( );
-		
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		for ( int i=0; i<nV; ++i )
-			row[i] = A[number*NVMAX + i];
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	g e t L B A
- */
-inline returnValue QProblem::getLBA( real_t* const _lbA ) const
-{
-	int i;
-
-	for ( i=0; i<getNC( ); ++i )
-		_lbA[i] = lbA[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t L B A
- */
-inline returnValue QProblem::getLBA( int number, real_t& value ) const
-{
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		value = lbA[number];
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	g e t U B A
- */
-inline returnValue QProblem::getUBA( real_t* const _ubA ) const
-{
-	int i;
-
-	for ( i=0; i<getNC( ); ++i )
-		_ubA[i] = ubA[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t U B A
- */
-inline returnValue QProblem::getUBA( int number, real_t& value ) const
-{
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		value = ubA[number];
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	g e t C o n s t r a i n t s
- */
-inline returnValue QProblem::getConstraints( Constraints* const _constraints ) const
-{
-	*_constraints = constraints;
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	g e t N C
- */
-inline int QProblem::getNC( ) const
-{
-	return constraints.getNC( );
-}
-
-
-/*
- *	g e t N E C
- */
-inline int QProblem::getNEC( ) const
-{
-	return constraints.getNEC( );
-}
-
-
-/*
- *	g e t N A C
- */
-inline int QProblem::getNAC( )
-{
-	return constraints.getNAC( );
-}
-
-
-/*
- *	g e t N I A C
- */
-inline int QProblem::getNIAC( )
-{
-	return constraints.getNIAC( );
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
- 
-
-/*
- *	s e t A
- */
-inline returnValue QProblem::setA( const real_t* const A_new )
-{
-	int i, j;
-	int nV = getNV( );
-	int nC = getNC( );
-
-	/* Set constraint matrix AND update member AX. */
-	for( j=0; j<nC; ++j )
-	{
-		Ax[j] = 0.0;
-
-		for( i=0; i<nV; ++i )
-		{	
-			A[j*NVMAX + i] = A_new[j*nV + i];
-			Ax[j] += A[j*NVMAX + i] * x[i];
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t A
- */
-inline returnValue QProblem::setA( int number, const real_t* const row )
-{
-	int i;
-	int nV = getNV( );
-
-	/* Set constraint matrix AND update member AX. */
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		Ax[number] = 0.0;
-
-		for( i=0; i<nV; ++i )
-		{
-			A[number*NVMAX + i] = row[i];
-			Ax[number] += A[number*NVMAX + i] * x[i];
-		}
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t L B A
- */
-inline returnValue QProblem::setLBA( const real_t* const lbA_new )
-{
-	int i;
-	int nC = getNC();
-
-	for( i=0; i<nC; ++i )
-		lbA[i] = lbA_new[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B A
- */
-inline returnValue QProblem::setLBA( int number, real_t value )
-{
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		lbA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t U B A
- */
-inline returnValue QProblem::setUBA( const real_t* const ubA_new )
-{
-	int i;
-	int nC = getNC();
-
-	for( i=0; i<nC; ++i )
-		ubA[i] = ubA_new[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B A
- */
-inline returnValue QProblem::setUBA( int number, real_t value )
-{
-	if ( ( number >= 0 ) && ( number < getNC( ) ) )
-	{
-		ubA[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/QProblemB.cpp b/phonelibs/qpoases/SRC/QProblemB.cpp
deleted file mode 100644
index 47ac3536f..000000000
--- a/phonelibs/qpoases/SRC/QProblemB.cpp
+++ /dev/null
@@ -1,2151 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/QProblemB.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the QProblemB class which is able to use the newly
- *	developed online active set strategy for parametric quadratic programming.
- */
-
-
-#include <QProblemB.hpp>
-
-#include <stdio.h>
-
-void printmatrix(char *name, double *A, int m, int n) {
-  int i, j;
-
-  printf("%s = [...\n", name);
-  for (i = 0; i < m; i++) {
-    for (j = 0; j < n; j++)
-        printf("  % 9.4f", A[i*n+j]);
-    printf(",\n");
-  }
-  printf("];\n");
-}
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( )
-{
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	hasHessian = BT_FALSE;
-
-	bounds.init( 0 );
-
-	hasCholesky = BT_FALSE;
-
-	tau = 0.0;
-
-	hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
-	infeasible = BT_FALSE;
-	unbounded = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	#ifdef PC_DEBUG
-	printlevel = PL_MEDIUM;
-	setPrintLevel( PL_MEDIUM );
-	#else
-	printlevel = QPOASES_PRINTLEVEL;
-	#endif
-
-	count = 0;
-}
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( int _nV )
-{
-	/* consistency check */
-	if ( _nV <= 0 )
-	{
-		_nV = 1;
-		THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-	hasHessian = BT_FALSE;
-
-	/* reset global message handler */
-	getGlobalMessageHandler( )->reset( );
-
-	bounds.init( _nV );
-
-	hasCholesky = BT_FALSE;
-
-	tau = 0.0;
-
-	hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
-	infeasible = BT_FALSE;
-	unbounded = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	#ifdef PC_DEBUG
-	printlevel = PL_MEDIUM;
-	setPrintLevel( PL_MEDIUM );
-	#else
-	printlevel = QPOASES_PRINTLEVEL;
-	#endif
-
-	count = 0;
-}
-
-
-/*
- *	Q P r o b l e m B
- */
-QProblemB::QProblemB( const QProblemB& rhs )
-{
-	int i, j;
-
-	int _nV = rhs.bounds.getNV( );
-
-	for( i=0; i<_nV; ++i )
-		for( j=0; j<_nV; ++j )
-			H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
-
-	hasHessian = rhs.hasHessian;
-
-	for( i=0; i<_nV; ++i )
-		g[i] = rhs.g[i];
-
-	for( i=0; i<_nV; ++i )
-		lb[i] = rhs.lb[i];
-
-	for( i=0; i<_nV; ++i )
-		ub[i] = rhs.ub[i];
-
-
-	bounds = rhs.bounds;
-
-	for( i=0; i<_nV; ++i )
-		for( j=0; j<_nV; ++j )
-			R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
-	hasCholesky = rhs.hasCholesky;
-
-	for( i=0; i<_nV; ++i )
-		x[i] = rhs.x[i];
-
-	for( i=0; i<_nV; ++i )
-		y[i] = rhs.y[i];
-
-	tau = rhs.tau;
-
-	hessianType = rhs.hessianType;
-	infeasible = rhs.infeasible;
-	unbounded = rhs.unbounded;
-
-	status = rhs.status;
-
-	printlevel = rhs.printlevel;
-
-	count = rhs.count;
-}
-
-
-/*
- *	~ Q P r o b l e m B
- */
-QProblemB::~QProblemB( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-QProblemB& QProblemB::operator=( const QProblemB& rhs )
-{
-	int i, j;
-
-	if ( this != &rhs )
-	{
-		int _nV = rhs.bounds.getNV( );
-
-		for( i=0; i<_nV; ++i )
-			for( j=0; j<_nV; ++j )
-				H[i*NVMAX + j] = rhs.H[i*NVMAX + j];
-
-		hasHessian = rhs.hasHessian;
-
-		for( i=0; i<_nV; ++i )
-			g[i] = rhs.g[i];
-
-		for( i=0; i<_nV; ++i )
-			lb[i] = rhs.lb[i];
-
-		for( i=0; i<_nV; ++i )
-			ub[i] = rhs.ub[i];
-
-		bounds = rhs.bounds;
-
-		for( i=0; i<_nV; ++i )
-			for( j=0; j<_nV; ++j )
-				R[i*NVMAX + j] = rhs.R[i*NVMAX + j];
-		hasCholesky = rhs.hasCholesky;
-
-
-		for( i=0; i<_nV; ++i )
-			x[i] = rhs.x[i];
-
-		for( i=0; i<_nV; ++i )
-			y[i] = rhs.y[i];
-
-		tau = rhs.tau;
-
-		hessianType = rhs.hessianType;
-		infeasible = rhs.infeasible;
-		unbounded = rhs.unbounded;
-
-		status = rhs.status;
-
-		printlevel = rhs.printlevel;
-		setPrintLevel( rhs.printlevel );
-
-		count = rhs.count;
-	}
-
-	return *this;
-}
-
-
-/*
- *	r e s e t
- */
-returnValue QProblemB::reset( )
-{
-	int i, j;
-	int nV = getNV( );
-
-	/** 0) Reset has Hessian flag. */
-	hasHessian = BT_FALSE;
-
-	/* 1) Reset bounds. */
-	bounds.init( nV );
-
-	/* 2) Reset Cholesky decomposition. */
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			R[i*NVMAX + j] = 0.0;
-	hasCholesky = BT_FALSE;
-
-	/* 3) Reset steplength and status flags. */
-	tau = 0.0;
-
-	hessianType = HST_POSDEF_NULLSPACE; /* Hessian is assumed to be positive definite by default */
-	infeasible = BT_FALSE;
-	unbounded = BT_FALSE;
-
-	status = QPS_NOTINITIALISED;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	i n i t
- */
-returnValue QProblemB::init(	const real_t* const _H, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, const real_t* const yOpt, real_t* const cputime
-								)
-{
-	/* 1) Setup QP data. */
-	if (setupQPdata(_H, 0, _g, _lb, _ub) != SUCCESSFUL_RETURN)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP(0, yOpt, 0, nWSR, cputime);
-}
-
-returnValue QProblemB::init(	const real_t* const _H, const real_t* const _R, const real_t* const _g,
-								const real_t* const _lb, const real_t* const _ub,
-								int& nWSR, const real_t* const yOpt, real_t* const cputime
-								)
-{
-	/* 1) Setup QP data. */
-	if (setupQPdata(_H, _R, _g, _lb, _ub) != SUCCESSFUL_RETURN)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Call to main initialisation routine (without any additional information). */
-	return solveInitialQP(0, yOpt, 0, nWSR, cputime);
-}
-
-
-/*
- *	h o t s t a r t
- */
-returnValue QProblemB::hotstart(	const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
-									int& nWSR, real_t* const cputime
-									)
-{
-	int l;
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )       ||
-		 ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) ||
-		 ( getStatus( ) == QPS_PERFORMINGHOMOTOPY )   )
-	{
-		return THROWERROR( RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED );
-	}
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	/* I) PREPARATIONS */
-	/* 1) Reset status flags and increase QP counter. */
-	infeasible = BT_FALSE;
-	unbounded = BT_FALSE;
-
-	++count;
-
-	/* 2) Allocate delta vectors of gradient and bounds. */
-	returnValue returnvalue;
-	BooleanType Delta_bB_isZero;
-
-	int FR_idx[NVMAX];
-	int FX_idx[NVMAX];
-
-	real_t delta_g[NVMAX];
-	real_t delta_lb[NVMAX];
-	real_t delta_ub[NVMAX];
-
-	real_t delta_xFR[NVMAX];
-	real_t delta_xFX[NVMAX];
-	real_t delta_yFX[NVMAX];
-
-	int BC_idx;
-	SubjectToStatus BC_status;
-
-	#ifdef PC_DEBUG
-	char messageString[80];
-	#endif
-
-	/* II) MAIN HOMOTOPY LOOP */
-	for( l=0; l<nWSR; ++l )
-	{
-		status = QPS_PERFORMINGHOMOTOPY;
-
-		if ( printlevel == PL_HIGH )
-		{
-			#ifdef PC_DEBUG
-			sprintf( messageString,"%d ...",l );
-			getGlobalMessageHandler( )->throwInfo( RET_ITERATION_STARTED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			#endif
-		}
-
-		/* 1) Setup index arrays. */
-		if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		if ( bounds.getFixed( )->getNumberArray( FX_idx ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_HOTSTART_FAILED );
-
-		/* 2) Initialize shift direction of the gradient and the bounds. */
-		returnvalue = hotstart_determineDataShift(  FX_idx,
-													g_new,lb_new,ub_new,
-													delta_g,delta_lb,delta_ub,
-													Delta_bB_isZero
-													);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_SHIFT_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 3) Determination of step direction of X and Y. */
-		returnvalue = hotstart_determineStepDirection(	FR_idx,FX_idx,
-														delta_g,delta_lb,delta_ub,
-														Delta_bB_isZero,
-														delta_xFX,delta_xFR,delta_yFX
-														);
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_STEPDIRECTION_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-
-		/* 4) Determination of step length TAU. */
-		returnvalue = hotstart_determineStepLength(	FR_idx,FX_idx,
-													delta_lb,delta_ub,
-													delta_xFR,delta_yFX,
-													BC_idx,BC_status );
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-			THROWERROR( RET_STEPLENGTH_DETERMINATION_FAILED );
-			return returnvalue;
-		}
-
-		/* 5) Realization of the homotopy step. */
-		returnvalue = hotstart_performStep(	FR_idx,FX_idx,
-											delta_g,delta_lb,delta_ub,
-											delta_xFX,delta_xFR,delta_yFX,
-											BC_idx,BC_status
-											);
-
-
-		if ( returnvalue != SUCCESSFUL_RETURN )
-		{
-			nWSR = l;
-
-			/* stop runtime measurement */
-			if ( cputime != 0 )
-				*cputime = getCPUtime( ) - starttime;
-
-			/* optimal solution found? */
-			if ( returnvalue == RET_OPTIMAL_SOLUTION_FOUND )
-			{
-				status = QPS_SOLVED;
-
-				if ( printlevel == PL_HIGH )
-					THROWINFO( RET_OPTIMAL_SOLUTION_FOUND );
-
-				#ifdef PC_DEBUG
-	 			if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
-					THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-				#endif
-
-				/* check KKT optimality conditions */
-				return checkKKTconditions( );
-			}
-			else
-			{
-				/* checks for infeasibility... */
-				if ( infeasible == BT_TRUE )
-				{
-					status = QPS_HOMOTOPYQPSOLVED;
-					return THROWERROR( RET_HOTSTART_STOPPED_INFEASIBILITY );
-				}
-
-				/* ...unboundedness... */
-				if ( unbounded == BT_TRUE ) /* not necessary since objective function convex! */
-					return THROWERROR( RET_HOTSTART_STOPPED_UNBOUNDEDNESS );
-
-				/* ... and throw unspecific error otherwise */
-				THROWERROR( RET_HOMOTOPY_STEP_FAILED );
-				return returnvalue;
-			}
-		}
-
-		/* 6) Output information of successful QP iteration. */
-		status = QPS_HOMOTOPYQPSOLVED;
-
-		#ifdef PC_DEBUG
-		if ( printIteration( l,BC_idx,BC_status ) != SUCCESSFUL_RETURN )
-			THROWERROR( RET_PRINT_ITERATION_FAILED ); /* do not pass this as return value! */
-		#endif
-	}
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-
-	/* if programm gets to here, output information that QP could not be solved
-	 * within the given maximum numbers of working set changes */
-	if ( printlevel == PL_HIGH )
-	{
-		#ifdef PC_DEBUG
-		sprintf( messageString,"(nWSR = %d)",nWSR );
-		return getGlobalMessageHandler( )->throwWarning( RET_MAX_NWSR_REACHED,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-	/* Finally check KKT optimality conditions. */
-	returnValue returnvalueKKTcheck = checkKKTconditions( );
-
-	if ( returnvalueKKTcheck != SUCCESSFUL_RETURN )
-		return returnvalueKKTcheck;
-	else
-		return RET_MAX_NWSR_REACHED;
-}
-
-
-/*
- *	g e t N Z
- */
-int QProblemB::getNZ( )
-{
-	/* if no constraints are present: nZ=nFR */
-	return bounds.getFree( )->getLength( );
-}
-
-
-/*
- *	g e t O b j V a l
- */
-real_t QProblemB::getObjVal( ) const
-{
-	real_t objVal;
-
-	/* calculated optimal objective function value
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		objVal = getObjVal( x );
-	}
-	else
-	{
-		objVal = INFTY;
-	}
-
-	return objVal;
-}
-
-
-/*
- *	g e t O b j V a l
- */
-real_t QProblemB::getObjVal( const real_t* const _x ) const
-{
-	int i, j;
-	int nV = getNV( );
-
-	real_t obj_tmp = 0.0;
-
-	for( i=0; i<nV; ++i )
-	{
-		obj_tmp += _x[i]*g[i];
-
-		for( j=0; j<nV; ++j )
-			obj_tmp += 0.5*_x[i]*H[i*NVMAX + j]*_x[j];
-	}
-
-	return obj_tmp;
-}
-
-
-/*
- *	g e t P r i m a l S o l u t i o n
- */
-returnValue QProblemB::getPrimalSolution( real_t* const xOpt ) const
-{
-	int i;
-
-	/* return optimal primal solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		for( i=0; i<getNV( ); ++i )
-			xOpt[i] = x[i];
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-/*
- *	g e t D u a l S o l u t i o n
- */
-returnValue QProblemB::getDualSolution( real_t* const yOpt ) const
-{
-	int i;
-
-	/* return optimal dual solution vector
-	 * only if current QP has been solved */
-	if ( ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED ) )
-	{
-		for( i=0; i<getNV( ); ++i )
-			yOpt[i] = y[i];
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return RET_QP_NOT_SOLVED;
-	}
-}
-
-
-/*
- *	s e t P r i n t L e v e l
- */
-returnValue QProblemB::setPrintLevel( PrintLevel _printlevel )
-{
-	#ifndef __MATLAB__
-	if ( ( printlevel >= PL_MEDIUM ) && ( printlevel != _printlevel ) )
-		THROWINFO( RET_PRINTLEVEL_CHANGED );
-	#endif
-
-	printlevel = _printlevel;
-
-	/* update message handler preferences */
- 	switch ( printlevel )
- 	{
- 		case PL_NONE:
- 			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
-			break;
-
-		case PL_LOW:
- 			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_HIDDEN );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_HIDDEN );
-			break;
-
- 		default: /* PL_MEDIUM, PL_HIGH */
- 			getGlobalMessageHandler( )->setErrorVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setWarningVisibilityStatus( VS_VISIBLE );
-			getGlobalMessageHandler( )->setInfoVisibilityStatus( VS_VISIBLE );
-			break;
- 	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	c h e c k F o r I d e n t i t y H e s s i a n
- */
-returnValue QProblemB::checkForIdentityHessian( )
-{
-	int i, j;
-	int nV = getNV( );
-
-	/* nothing to do as status flag remains unaltered
-	 * if Hessian differs from identity matrix */
-	if ( hessianType == HST_IDENTITY )
-		return SUCCESSFUL_RETURN;
-
-	/* 1) If Hessian differs from identity matrix,
-	 *    return without changing the internal HessianType. */
-	for ( i=0; i<nV; ++i )
-		if ( getAbs( H[i*NVMAX + i] - 1.0 ) > EPS )
-			return SUCCESSFUL_RETURN;
-
-	for ( i=0; i<nV; ++i )
-	{
-		for ( j=0; j<i; ++j )
-			if ( ( getAbs( H[i*NVMAX + j] ) > EPS ) || ( getAbs( H[j*NVMAX + i] ) > EPS ) )
-				return SUCCESSFUL_RETURN;
-	}
-
-	/* 2) If this point is reached, Hessian equals the idetity matrix. */
-	hessianType = HST_IDENTITY;
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p S u b j e c t T o T y p e
- */
-returnValue QProblemB::setupSubjectToType( )
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* 1) Check if lower bounds are present. */
-	bounds.setNoLower( BT_TRUE );
-	for( i=0; i<nV; ++i )
-		if ( lb[i] > -INFTY )
-		{
-			bounds.setNoLower( BT_FALSE );
-			break;
-		}
-
-	/* 2) Check if upper bounds are present. */
-	bounds.setNoUpper( BT_TRUE );
-	for( i=0; i<nV; ++i )
-		if ( ub[i] < INFTY )
-		{
-			bounds.setNoUpper( BT_FALSE );
-			break;
-		}
-
-	/* 3) Determine implicitly fixed and unbounded variables. */
-	int nFV = 0;
-	int nUV = 0;
-
-	for( i=0; i<nV; ++i )
-		if ( ( lb[i] < -INFTY + BOUNDTOL ) && ( ub[i] > INFTY - BOUNDTOL ) )
-		{
-			bounds.setType( i,ST_UNBOUNDED );
-			++nUV;
-		}
-		else
-		{
-			if ( lb[i] > ub[i] - BOUNDTOL )
-			{
-				bounds.setType( i,ST_EQUALITY );
-				++nFV;
-			}
-			else
-			{
-				bounds.setType( i,ST_BOUNDED );
-			}
-		}
-
-	/* 4) Set dimensions of bounds structure. */
-	bounds.setNFV( nFV );
-	bounds.setNUV( nUV );
-	bounds.setNBV( nV - nFV - nUV );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	c h o l e s k y D e c o m p o s i t i o n
- */
-returnValue QProblemB::setupCholeskyDecomposition( )
-{
-	int i, j, k, ii, jj;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-
-	/* If Hessian flag is false, it means that H & R already contain Cholesky
-	 * factorization -- provided from outside. */
-	if (hasHessian == BT_FALSE)
-		return SUCCESSFUL_RETURN;
-
-	/* 1) Initialises R with all zeros. */
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			R[i*NVMAX + j] = 0.0;
-
-	/* 2) Calculate Cholesky decomposition of H (projected to free variables). */
-	if ( hessianType == HST_IDENTITY )
-	{
-		/* if Hessian is identity, so is its Cholesky factor. */
-		for( i=0; i<nFR; ++i )
-			R[i*NVMAX + i] = 1.0;
-	}
-	else
-	{
-		if ( nFR > 0 )
-		{
-			int FR_idx[NVMAX];
-			if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_INDEXLIST_CORRUPTED );
-
-			/* R'*R = H */
-			real_t sum;
-			real_t inv;
-
-			for( i=0; i<nFR; ++i )
-			{
-				/* j == i */
-				ii = FR_idx[i];
-				sum = H[ii*NVMAX + ii];
-
-				for( k=(i-1); k>=0; --k )
-					sum -= R[k*NVMAX + i] * R[k*NVMAX + i];
-
-				if ( sum > 0.0 )
-				{
-					R[i*NVMAX + i] = sqrt( sum );
-					inv = 1.0 / R[i*NVMAX + i];
-				}
-				else
-				{
-					hessianType = HST_SEMIDEF;
-					return THROWERROR( RET_HESSIAN_NOT_SPD );
-				}
-
-				/* j > i */
-				for( j=(i+1); j<nFR; ++j )
-				{
-					jj = FR_idx[j];
-					sum = H[jj*NVMAX + ii];
-
-					for( k=(i-1); k>=0; --k )
-						sum -= R[k*NVMAX + i] * R[k*NVMAX + j];
-
-					R[i*NVMAX + j] = sum * inv;
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s o l v e I n i t i a l Q P
- */
-returnValue QProblemB::solveInitialQP(	const real_t* const xOpt, const real_t* const yOpt,
-										const Bounds* const guessedBounds,
-										int& nWSR, real_t* const cputime
-										)
-{
-	int i, nFR;
-	int nV = getNV( );
-
-
-	/* start runtime measurement */
-	real_t starttime = 0.0;
-	if ( cputime != 0 )
-		starttime = getCPUtime( );
-
-
-	status = QPS_NOTINITIALISED;
-
-	/* I) ANALYSE QP DATA: */
-	/* 1) Check if Hessian happens to be the identity matrix. */
-	if ( checkForIdentityHessian( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup type of bounds (i.e. unbounded, implicitly fixed etc.). */
-	if ( setupSubjectToType( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	status = QPS_PREPARINGAUXILIARYQP;
-
-
-	/* II) SETUP AUXILIARY QP WITH GIVEN OPTIMAL SOLUTION: */
-	/* 1) Setup bounds data structure. */
-	if ( bounds.setupAllFree( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 2) Setup optimal primal/dual solution for auxiliary QP. */
-	if ( setupAuxiliaryQPsolution( xOpt,yOpt ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 3) Obtain linear independent working set for auxiliary QP. */
-
-	static Bounds auxiliaryBounds;
-
-	auxiliaryBounds.init( nV );
-
-	if ( obtainAuxiliaryWorkingSet( xOpt,yOpt,guessedBounds, &auxiliaryBounds ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	/* 4) Setup working set of auxiliary QP and setup cholesky decomposition. */
-	if ( setupAuxiliaryWorkingSet( &auxiliaryBounds,BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	nFR = getNFR();
-	/* At the moment we can only provide a Cholesky of the Hessian if
-	 * the solver is cold-started. */
-	if (hasCholesky == BT_FALSE || nFR != nV)
-		if (setupCholeskyDecomposition() != SUCCESSFUL_RETURN)
-			return THROWERROR( RET_INIT_FAILED_CHOLESKY );
-
-	/* 5) Store original QP formulation... */
-	real_t g_original[NVMAX];
-	real_t lb_original[NVMAX];
-	real_t ub_original[NVMAX];
-
-	for( i=0; i<nV; ++i )
-		g_original[i] = g[i];
-	for( i=0; i<nV; ++i )
-		lb_original[i] = lb[i];
-	for( i=0; i<nV; ++i )
-		ub_original[i] = ub[i];
-
-	/* ... and setup QP data of an auxiliary QP having an optimal solution
-	 * as specified by the user (or xOpt = yOpt = 0, by default). */
-	if ( setupAuxiliaryQPgradient( ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	if ( setupAuxiliaryQPbounds( BT_TRUE ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_INIT_FAILED );
-
-	status = QPS_AUXILIARYQPSOLVED;
-
-
-	/* III) SOLVE ACTUAL INITIAL QP: */
-	/* Use hotstart method to find the solution of the original initial QP,... */
-	returnValue returnvalue = hotstart( g_original,lb_original,ub_original, nWSR,0 );
-
-
-	/* ... check for infeasibility and unboundedness... */
-	if ( isInfeasible( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_INFEASIBILITY );
-
-	if ( isUnbounded( ) == BT_TRUE )
-		return THROWERROR( RET_INIT_FAILED_UNBOUNDEDNESS );
-
-	/* ... and internal errors. */
-	if ( ( returnvalue != SUCCESSFUL_RETURN ) && ( returnvalue != RET_MAX_NWSR_REACHED )  &&
-	     ( returnvalue != RET_INACCURATE_SOLUTION ) && ( returnvalue != RET_NO_SOLUTION ) )
-		return THROWERROR( RET_INIT_FAILED_HOTSTART );
-
-
-	/* stop runtime measurement */
-	if ( cputime != 0 )
-		*cputime = getCPUtime( ) - starttime;
-
-	if ( printlevel == PL_HIGH )
-		THROWINFO( RET_INIT_SUCCESSFUL );
-
-	return returnvalue;
-}
-
-
-/*
- *	o b t a i n A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblemB::obtainAuxiliaryWorkingSet(	const real_t* const xOpt, const real_t* const yOpt,
-													const Bounds* const guessedBounds, Bounds* auxiliaryBounds
-													) const
-{
-	int i = 0;
-	int nV = getNV( );
-
-
-	/* 1) Ensure that desiredBounds is allocated (and different from guessedBounds). */
-	if ( ( auxiliaryBounds == 0 ) || ( auxiliaryBounds == guessedBounds ) )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-
-	/* 2) Setup working set for auxiliary initial QP. */
-	if ( guessedBounds != 0 )
-	{
-		/* If an initial working set is specific, use it!
-		 * Moreover, add all implictly fixed variables if specified. */
-		for( i=0; i<nV; ++i )
-		{
-			if ( bounds.getType( i ) == ST_EQUALITY )
-			{
-				if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-			else
-			{
-				if ( auxiliaryBounds->setupBound( i,guessedBounds->getStatus( i ) ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-			}
-		}
-	}
-	else	/* No initial working set specified. */
-	{
-		if ( ( xOpt != 0 ) && ( yOpt == 0 ) )
-		{
-			/* Obtain initial working set by "clipping". */
-			for( i=0; i<nV; ++i )
-			{
-				if ( xOpt[i] <= lb[i] + BOUNDTOL )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( xOpt[i] >= ub[i] - BOUNDTOL )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all implictly fixed variables if specified. */
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		if ( ( xOpt == 0 ) && ( yOpt != 0 ) )
-		{
-			/* Obtain initial working set in accordance to sign of dual solution vector. */
-			for( i=0; i<nV; ++i )
-			{
-				if ( yOpt[i] > ZERO )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				if ( yOpt[i] < -ZERO )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_UPPER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-					continue;
-				}
-
-				/* Moreover, add all implictly fixed variables if specified. */
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-
-		/* If xOpt and yOpt are null pointer and no initial working is specified,
-		 * start with empty working set (or implicitly fixed bounds only)
-		 * for auxiliary QP. */
-		if ( ( xOpt == 0 ) && ( yOpt == 0 ) )
-		{
-			for( i=0; i<nV; ++i )
-			{
-				/* Only add all implictly fixed variables if specified. */
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_LOWER ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-				else
-				{
-					if ( auxiliaryBounds->setupBound( i,ST_INACTIVE ) != SUCCESSFUL_RETURN )
-						return THROWERROR( RET_OBTAINING_WORKINGSET_FAILED );
-				}
-			}
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y W o r k i n g S e t
- */
-returnValue QProblemB::setupAuxiliaryWorkingSet( 	const Bounds* const auxiliaryBounds,
-													BooleanType setupAfresh
-													)
-{
-	int i;
-	int nV = getNV( );
-
-	/* consistency checks */
-	if ( auxiliaryBounds != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			if ( ( bounds.getStatus( i ) == ST_UNDEFINED ) || ( auxiliaryBounds->getStatus( i ) == ST_UNDEFINED ) )
-				return THROWERROR( RET_UNKNOWN_BUG );
-	}
-	else
-	{
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-	}
-
-
-	/* I) SETUP CHOLESKY FLAG:
-	 *    Cholesky decomposition shall only be updated if working set
-	 *    shall be updated (i.e. NOT setup afresh!) */
-	BooleanType updateCholesky;
-	if ( setupAfresh == BT_TRUE )
-		updateCholesky = BT_FALSE;
-	else
-		updateCholesky = BT_TRUE;
-
-
-	/* II) REMOVE FORMERLY ACTIVE BOUNDS (IF NECESSARY): */
-	if ( setupAfresh == BT_FALSE )
-	{
-		/* Remove all active bounds that shall be inactive AND
-		*  all active bounds that are active at the wrong bound. */
-		for( i=0; i<nV; ++i )
-		{
-			if ( ( bounds.getStatus( i ) == ST_LOWER ) && ( auxiliaryBounds->getStatus( i ) != ST_LOWER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-
-			if ( ( bounds.getStatus( i ) == ST_UPPER ) && ( auxiliaryBounds->getStatus( i ) != ST_UPPER ) )
-				if ( removeBound( i,updateCholesky ) != SUCCESSFUL_RETURN )
-					return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-
-	/* III) ADD NEWLY ACTIVE BOUNDS: */
-	/*      Add all inactive bounds that shall be active AND
-	 *      all formerly active bounds that have been active at the wrong bound. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( ( bounds.getStatus( i ) == ST_INACTIVE ) && ( auxiliaryBounds->getStatus( i ) != ST_INACTIVE ) )
-		{
-			if ( addBound( i,auxiliaryBounds->getStatus( i ),updateCholesky ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_SETUP_WORKINGSET_FAILED );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P s o l u t i o n
- */
-returnValue QProblemB::setupAuxiliaryQPsolution(	const real_t* const xOpt, const real_t* const yOpt
-													)
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* Setup primal/dual solution vectors for auxiliary initial QP:
-	 * if a null pointer is passed, a zero vector is assigned;
-	 * old solution vector is kept if pointer to internal solution vector is passed. */
-	if ( xOpt != 0 )
-	{
-		if ( xOpt != x )
-			for( i=0; i<nV; ++i )
-				x[i] = xOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			x[i] = 0.0;
-	}
-
-	if ( yOpt != 0 )
-	{
-		if ( yOpt != y )
-			for( i=0; i<nV; ++i )
-				y[i] = yOpt[i];
-	}
-	else
-	{
-		for( i=0; i<nV; ++i )
-			y[i] = 0.0;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P g r a d i e n t
- */
-returnValue QProblemB::setupAuxiliaryQPgradient( )
-{
-	int i, j;
-	int nV = getNV( );
-
-
-	/* Setup gradient vector: g = -H*x + y'*Id. */
-	for ( i=0; i<nV; ++i )
-	{
-		/* y'*Id */
-		g[i] = y[i];
-
-		/* -H*x */
-		for ( j=0; j<nV; ++j )
-			g[i] -= H[i*NVMAX + j] * x[j];
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t u p A u x i l i a r y Q P b o u n d s
- */
-returnValue QProblemB::setupAuxiliaryQPbounds( BooleanType useRelaxation )
-{
-	int i;
-	int nV = getNV( );
-
-
-	/* Setup bound vectors. */
-	for ( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_INACTIVE:
-				if ( useRelaxation == BT_TRUE )
-				{
-					if ( bounds.getType( i ) == ST_EQUALITY )
-					{
-						lb[i] = x[i];
-						ub[i] = x[i];
-					}
-					else
-					{
-						lb[i] = x[i] - BOUNDRELAXATION;
-						ub[i] = x[i] + BOUNDRELAXATION;
-					}
-				}
-				break;
-
-			case ST_LOWER:
-				lb[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					ub[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						ub[i] = x[i] + BOUNDRELAXATION;
-				}
-				break;
-
-			case ST_UPPER:
-				ub[i] = x[i];
-				if ( bounds.getType( i ) == ST_EQUALITY )
-				{
-					lb[i] = x[i];
-				}
-				else
-				{
-					if ( useRelaxation == BT_TRUE )
-						lb[i] = x[i] - BOUNDRELAXATION;
-				}
-				break;
-
-			default:
-				return THROWERROR( RET_UNKNOWN_BUG );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a d d B o u n d
- */
-returnValue QProblemB::addBound(	int number, SubjectToStatus B_status,
-									BooleanType updateCholesky
-									)
-{
-	int i, j;
-	int nFR = getNFR( );
-
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-	/* Perform cholesky updates only if QProblemB has been initialised! */
-	if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
-	{
-		/* UPDATE INDICES */
-		if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_ADDBOUND_FAILED );
-
-		return SUCCESSFUL_RETURN;
-	}
-
-
-	/* I) PERFORM CHOLESKY UPDATE: */
-	/* 1) Index of variable to be added within the list of free variables. */
-	int number_idx = bounds.getFree( )->getIndex( number );
-
-	real_t c, s;
-
-	/* 2) Use row-wise Givens rotations to restore upper triangular form of R. */
-	for( i=number_idx+1; i<nFR; ++i )
-	{
-		computeGivens( R[(i-1)*NVMAX + i],R[i*NVMAX + i], R[(i-1)*NVMAX + i],R[i*NVMAX + i],c,s );
-
-		for( j=(1+i); j<nFR; ++j ) /* last column of R is thrown away */
-			applyGivens( c,s,R[(i-1)*NVMAX + j],R[i*NVMAX + j], R[(i-1)*NVMAX + j],R[i*NVMAX + j] );
-	}
-
-	/* 3) Delete <number_idx>th column and ... */
-	for( i=0; i<nFR-1; ++i )
-		for( j=number_idx+1; j<nFR; ++j )
-			R[i*NVMAX + j-1] = R[i*NVMAX + j];
-	/* ... last column of R. */
-	for( i=0; i<nFR; ++i )
-		R[i*NVMAX + nFR-1] = 0.0;
-
-
-	/* II) UPDATE INDICES */
-	if ( bounds.moveFreeToFixed( number,B_status ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_ADDBOUND_FAILED );
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-returnValue QProblemB::removeBound(	int number,
-									BooleanType updateCholesky
-									)
-{
-	int i, ii;
-	int nFR = getNFR( );
-
-
-	/* consistency check */
-	if ( ( getStatus( ) == QPS_NOTINITIALISED )    ||
-		 ( getStatus( ) == QPS_AUXILIARYQPSOLVED ) ||
-		 ( getStatus( ) == QPS_HOMOTOPYQPSOLVED )  ||
-		 ( getStatus( ) == QPS_SOLVED )            )
-	{
-		return THROWERROR( RET_UNKNOWN_BUG );
-	}
-
-
-	/* I) UPDATE INDICES */
-	if ( bounds.moveFixedToFree( number ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	/* Perform cholesky updates only if QProblemB has been initialised! */
-	if ( ( getStatus( ) == QPS_PREPARINGAUXILIARYQP ) || ( updateCholesky == BT_FALSE ) )
-		return SUCCESSFUL_RETURN;
-
-
-	/* II) PERFORM CHOLESKY UPDATE */
-	int FR_idx[NVMAX];
-	if ( bounds.getFree( )->getNumberArray( FR_idx ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	/* 1) Calculate new column of cholesky decomposition. */
-	real_t rhs[NVMAX];
-	real_t r[NVMAX];
-	real_t r0 = H[number*NVMAX + number];
-
-	for( i=0; i<nFR; ++i )
-	{
-		ii = FR_idx[i];
-		rhs[i] = H[number*NVMAX + ii];
-	}
-
-	if ( backsolveR( rhs,BT_TRUE,BT_TRUE,r ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_REMOVEBOUND_FAILED );
-
-	for( i=0; i<nFR; ++i )
-		r0 -= r[i]*r[i];
-
-	/* 2) Store new column into R. */
-	for( i=0; i<nFR; ++i )
-		R[i*NVMAX + nFR] = r[i];
-
-	if ( r0 > 0.0 )
-		R[nFR*NVMAX + nFR] = sqrt( r0 );
-	else
-	{
-		hessianType = HST_SEMIDEF;
-		return THROWERROR( RET_HESSIAN_NOT_SPD );
-	}
-
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	b a c k s o l v e R  (CODE DUPLICATED IN QProblem CLASS!!!)
- */
-returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
-									real_t* const a
-									)
-{
-	/* Call standard backsolve procedure (i.e. removingBound == BT_FALSE). */
-	return backsolveR( b,transposed,BT_FALSE,a );
-}
-
-
-/*
- *	b a c k s o l v e R  (CODE DUPLICATED IN QProblem CLASS!!!)
- */
-returnValue QProblemB::backsolveR(	const real_t* const b, BooleanType transposed,
-									BooleanType removingBound,
-									real_t* const a
-									)
-{
-	int i, j;
-	int nR = getNZ( );
-
-	real_t sum;
-
-	/* if backsolve is called while removing a bound, reduce nZ by one. */
-	if ( removingBound == BT_TRUE )
-		--nR;
-
-	/* nothing to do */
-	if ( nR <= 0 )
-		return SUCCESSFUL_RETURN;
-
-
-	/* Solve Ra = b, where R might be transposed. */
-	if ( transposed == BT_FALSE )
-	{
-		/* solve Ra = b */
-		for( i=(nR-1); i>=0; --i )
-		{
-			sum = b[i];
-			for( j=(i+1); j<nR; ++j )
-				sum -= R[i*NVMAX + j] * a[j];
-
-			if ( getAbs( R[i*NVMAX + i] ) > ZERO )
-				a[i] = sum / R[i*NVMAX + i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-	else
-	{
-		/* solve R^T*a = b */
-		for( i=0; i<nR; ++i )
-		{
-			sum = b[i];
-
-			for( j=0; j<i; ++j )
-				sum -= R[j*NVMAX + i] * a[j];
-
-			if ( getAbs( R[i*NVMAX + i] ) > ZERO )
-				a[i] = sum / R[i*NVMAX + i];
-			else
-				return THROWERROR( RET_DIV_BY_ZERO );
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ d e t e r m i n e D a t a S h i f t
- */
-returnValue QProblemB::hotstart_determineDataShift(	const int* const FX_idx,
-													const real_t* const g_new, const real_t* const lb_new, const real_t* const ub_new,
-													real_t* const delta_g, real_t* const delta_lb, real_t* const delta_ub,
-													BooleanType& Delta_bB_isZero
-													)
-{
-	int i, ii;
-	int nV  = getNV( );
-	int nFX = getNFX( );
-
-
-	/* 1) Calculate shift directions. */
-	for( i=0; i<nV; ++i )
-		delta_g[i]  = g_new[i]  - g[i];
-
-	if ( lb_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			delta_lb[i] = lb_new[i] - lb[i];
-	}
-	else
-	{
-		/* if no lower bounds exist, assume the new lower bounds to be -infinity */
-		for( i=0; i<nV; ++i )
-			delta_lb[i] = -INFTY - lb[i];
-	}
-
-	if ( ub_new != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			delta_ub[i] = ub_new[i] - ub[i];
-	}
-	else
-	{
-		/* if no upper bounds exist, assume the new upper bounds to be infinity */
-		for( i=0; i<nV; ++i )
-			delta_ub[i] = INFTY - ub[i];
-	}
-
-	/* 2) Determine if active bounds are to be shifted. */
-	Delta_bB_isZero = BT_TRUE;
-
-	for ( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( ( getAbs( delta_lb[ii] ) > EPS ) || ( getAbs( delta_ub[ii] ) > EPS ) )
-		{
-			Delta_bB_isZero = BT_FALSE;
-			break;
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	a r e B o u n d s C o n s i s t e n t
- */
-BooleanType QProblemB::areBoundsConsistent(	const real_t* const delta_lb, const real_t* const delta_ub
-											) const
-{
-	int i;
-
-	/* Check if delta_lb[i] is greater than delta_ub[i]
-	 * for a component i whose bounds are already (numerically) equal. */
-	for( i=0; i<getNV( ); ++i )
-		if ( ( lb[i] > ub[i] - BOUNDTOL ) && ( delta_lb[i] > delta_ub[i] + EPS ) )
-			return BT_FALSE;
-
-	return BT_TRUE;
-}
-
-
-/*
- *	s e t u p Q P d a t a
- */
-returnValue QProblemB::setupQPdata(	const real_t* const _H, const real_t* const _R, const real_t* const _g,
-									const real_t* const _lb, const real_t* const _ub
-									)
-{
-	int i, j;
-	int nV = getNV( );
-
-	/* 1) Setup Hessian matrix and it's Cholesky factorization. */
-	if (_H != 0)
-	{
-		for( i=0; i<nV; ++i )
-			for( j=0; j<nV; ++j )
-				H[i*NVMAX + j] = _H[i*nV + j];
-		hasHessian = BT_TRUE;
-	}
-	else
-		hasHessian = BT_FALSE;
-
-	if (_R != 0)
-	{
-		for( i=0; i<nV; ++i )
-			for( j=0; j<nV; ++j )
-				R[i*NVMAX + j] = _R[i*nV + j];
-		hasCholesky = BT_TRUE;
-
-		/* If Hessian is not provided, store it's factorization in H, and that guy
-		 * is going to be used for H * x products (R^T * R * x in this case). */
-		if (hasHessian == BT_FALSE)
-			for( i=0; i<nV; ++i )
-				for( j=0; j<nV; ++j )
-					H[i*NVMAX + j] = _R[i*nV + j];
-	}
-	else
-		hasCholesky = BT_FALSE;
-
-	if (hasHessian == BT_FALSE && hasCholesky == BT_FALSE)
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* 2) Setup gradient vector. */
-	if ( _g == 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	for( i=0; i<nV; ++i )
-		g[i] = _g[i];
-
-	/* 3) Setup lower bounds vector. */
-	if ( _lb != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			lb[i] = _lb[i];
-	}
-	else
-	{
-		/* if no lower bounds are specified, set them to -infinity */
-		for( i=0; i<nV; ++i )
-			lb[i] = -INFTY;
-	}
-
-	/* 4) Setup upper bounds vector. */
-	if ( _ub != 0 )
-	{
-		for( i=0; i<nV; ++i )
-			ub[i] = _ub[i];
-	}
-	else
-	{
-		/* if no upper bounds are specified, set them to infinity */
-		for( i=0; i<nV; ++i )
-			ub[i] = INFTY;
-	}
-
-	//printmatrix( "H",H,nV,nV );
-	//printmatrix( "R",R,nV,nV );
-	//printmatrix( "g",g,1,nV );
-	//printmatrix( "lb",lb,1,nV );
-	//printmatrix( "ub",ub,1,nV );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R I V A T E                                                            *
- *****************************************************************************/
-
-/*
- *	h o t s t a r t _ d e t e r m i n e S t e p D i r e c t i o n
- */
-returnValue QProblemB::hotstart_determineStepDirection(	const int* const FR_idx, const int* const FX_idx,
-														const real_t* const delta_g, const real_t* const delta_lb, const real_t* const delta_ub,
-														BooleanType Delta_bB_isZero,
-														real_t* const delta_xFX, real_t* const delta_xFR,
-														real_t* const delta_yFX
-														)
-{
-	int i, j, ii, jj;
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-
-
-	/* initialise auxiliary vectors */
-	real_t HMX_delta_xFX[NVMAX];
-	for( i=0; i<nFR; ++i )
-		HMX_delta_xFX[i] = 0.0;
-
-
-	/* I) DETERMINE delta_xFX */
-	if ( nFX > 0 )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-				delta_xFX[i] = delta_lb[ii];
-			else
-				delta_xFX[i] = delta_ub[ii];
-		}
-	}
-
-
-	/* II) DETERMINE delta_xFR */
-	if ( nFR > 0 )
-	{
-		/* auxiliary variables */
-		real_t delta_xFRz_TMP[NVMAX];
-		real_t delta_xFRz_RHS[NVMAX];
-
-		/* Determine delta_xFRz. */
-		if ( Delta_bB_isZero == BT_FALSE )
-		{
-			for( i=0; i<nFR; ++i )
-			{
-				ii = FR_idx[i];
-				for( j=0; j<nFX; ++j )
-				{
-					jj = FX_idx[j];
-					HMX_delta_xFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
-				}
-			}
-		}
-
-		if ( Delta_bB_isZero == BT_TRUE )
-		{
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				delta_xFRz_RHS[j] = delta_g[jj];
-			}
-		}
-		else
-		{
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				delta_xFRz_RHS[j] = delta_g[jj] + HMX_delta_xFX[j]; /* *ZFR */
-			}
-		}
-
-		for( i=0; i<nFR; ++i )
-			delta_xFR[i] = -delta_xFRz_RHS[i];
-
-		if ( backsolveR( delta_xFR,BT_TRUE,delta_xFRz_TMP ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-
-		if ( backsolveR( delta_xFRz_TMP,BT_FALSE,delta_xFR ) != SUCCESSFUL_RETURN )
-			return THROWERROR( RET_STEPDIRECTION_FAILED_CHOLESKY );
-	}
-
-
-	/* III) DETERMINE delta_yFX */
-	if ( nFX > 0 )
-	{
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-
-			delta_yFX[i] = 0.0;
-			for( j=0; j<nFR; ++j )
-			{
-				jj = FR_idx[j];
-				delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFR[j];
-			}
-
-			if ( Delta_bB_isZero == BT_FALSE )
-			{
-				for( j=0; j<nFX; ++j )
-				{
-					jj = FX_idx[j];
-					delta_yFX[i] += H[ii*NVMAX + jj] * delta_xFX[j];
-				}
-			}
-
-			delta_yFX[i] += delta_g[ii];
-		}
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ d e t e r m i n e S t e p L e n g t h
- */
-returnValue QProblemB::hotstart_determineStepLength(	const int* const FR_idx, const int* const FX_idx,
-														const real_t* const delta_lb, const real_t* const delta_ub,
-														const real_t* const delta_xFR,
-														const real_t* const delta_yFX,
-														int& BC_idx, SubjectToStatus& BC_status
-														)
-{
-	int i, ii;
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-
-	real_t tau_tmp;
-	real_t tau_new = 1.0;
-
-	BC_idx = 0;
-	BC_status = ST_UNDEFINED;
-
-
-	/* I) DETERMINE MAXIMUM DUAL STEPLENGTH, i.e. ensure that
-	 *    active dual bounds remain valid (ignoring implicitly fixed variables): */
-	for( i=0; i<nFX; ++i )
-	{
-		ii = FX_idx[i];
-
-		if ( bounds.getType( ii ) != ST_EQUALITY )
-		{
-			if ( bounds.getStatus( ii ) == ST_LOWER )
-			{
-				/* 1) Active lower bounds. */
-				if ( ( delta_yFX[i] < -ZERO ) && ( y[ii] >= 0.0 ) )
-				{
-					tau_tmp = y[ii] / ( -delta_yFX[i] );
-					if ( tau_tmp < tau_new )
-					{
-						if ( tau_tmp >= 0.0 )
-						{
-							tau_new = tau_tmp;
-							BC_idx = ii;
-							BC_status = ST_INACTIVE;
-						}
-					}
-				}
-			}
-			else
-			{
-				/* 2) Active upper bounds. */
-				if ( ( delta_yFX[i] > ZERO ) && ( y[ii] <= 0.0 ) )
-				{
-					tau_tmp = y[ii] / ( -delta_yFX[i] );
-					if ( tau_tmp < tau_new )
-					{
-						if ( tau_tmp >= 0.0 )
-						{
-							tau_new = tau_tmp;
-							BC_idx = ii;
-							BC_status = ST_INACTIVE;
-						}
-					}
-				}
-			}
-		}
-	}
-
-
-	/* II) DETERMINE MAXIMUM PRIMAL STEPLENGTH, i.e. ensure that
-	 *     inactive bounds remain valid (ignoring unbounded variables). */
-	/* 1) Inactive lower bounds. */
-	if ( bounds.isNoLower( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-
-			if ( bounds.getType( ii ) != ST_UNBOUNDED )
-			{
-				if ( delta_lb[ii] > delta_xFR[i] )
-				{
-					if ( x[ii] > lb[ii] )
-						tau_tmp = ( x[ii] - lb[ii] ) / ( delta_lb[ii] - delta_xFR[i] );
-					else
-						tau_tmp = 0.0;
-
-					if ( tau_tmp < tau_new )
-					{
-						if ( tau_tmp >= 0.0 )
-						{
-							tau_new = tau_tmp;
-							BC_idx = ii;
-							BC_status = ST_LOWER;
-						}
-					}
-				}
-			}
-		}
-	}
-
-	/* 2) Inactive upper bounds. */
-	if ( bounds.isNoUpper( ) == BT_FALSE )
-	{
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-
-			if ( bounds.getType( ii ) != ST_UNBOUNDED )
-			{
-				if ( delta_ub[ii] < delta_xFR[i] )
-				{
-					if ( x[ii] < ub[ii] )
-						tau_tmp = ( x[ii] - ub[ii] ) / ( delta_ub[ii] - delta_xFR[i] );
-					else
-						tau_tmp = 0.0;
-
-					if ( tau_tmp < tau_new )
-					{
-						if ( tau_tmp >= 0.0 )
-						{
-							tau_new = tau_tmp;
-							BC_idx = ii;
-							BC_status = ST_UPPER;
-						}
-					}
-				}
-			}
-		}
-	}
-
-
-	/* III) SET MAXIMUM HOMOTOPY STEPLENGTH */
-	tau = tau_new;
-
-	if ( printlevel ==  PL_HIGH )
-	{
-		#ifdef PC_DEBUG
-		char messageString[80];
-
-		if ( BC_status == ST_UNDEFINED )
-			sprintf( messageString,"Stepsize is %.6e!",tau );
-		else
-			sprintf( messageString,"Stepsize is %.6e! (BC_idx = %d, BC_status = %d)",tau,BC_idx,BC_status );
-
-		getGlobalMessageHandler( )->throwInfo( RET_STEPSIZE_NONPOSITIVE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	h o t s t a r t _ p e r f o r m S t e p
- */
-returnValue QProblemB::hotstart_performStep(	const int* const FR_idx, const int* const FX_idx,
-												const real_t* const delta_g, const real_t* const  delta_lb, const real_t* const delta_ub,
-												const real_t* const delta_xFX, const real_t* const delta_xFR,
-												const real_t* const delta_yFX,
-												int BC_idx, SubjectToStatus BC_status
-												)
-{
-	int i, ii;
-	int nV  = getNV( );
-	int nFR = getNFR( );
-	int nFX = getNFX( );
-
-
-	/* I) CHECK BOUNDS' CONSISTENCY */
-	if ( areBoundsConsistent( delta_lb,delta_ub ) == BT_FALSE )
-	{
-		infeasible = BT_TRUE;
-		tau = 0.0;
-
-		return THROWERROR( RET_QP_INFEASIBLE );
-	}
-
-
-	/* II) GO TO ACTIVE SET CHANGE */
-	if ( tau > ZERO )
-	{
-		/* 1) Perform step in primal und dual space. */
-		for( i=0; i<nFR; ++i )
-		{
-			ii = FR_idx[i];
-			x[ii] += tau*delta_xFR[i];
-		}
-
-		for( i=0; i<nFX; ++i )
-		{
-			ii = FX_idx[i];
-			x[ii] += tau*delta_xFX[i];
-			y[ii] += tau*delta_yFX[i];
-		}
-
-		/* 2) Shift QP data. */
-		for( i=0; i<nV; ++i )
-		{
-			g[i]  += tau*delta_g[i];
-			lb[i] += tau*delta_lb[i];
-			ub[i] += tau*delta_ub[i];
-		}
-	}
-	else
-	{
-		/* print a stepsize warning if stepsize is zero */
-		#ifdef PC_DEBUG
-		char messageString[80];
-		sprintf( messageString,"Stepsize is %.6e",tau );
-		getGlobalMessageHandler( )->throwWarning( RET_STEPSIZE,messageString,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		#endif
-	}
-
-
-	/* setup output preferences */
-	#ifdef PC_DEBUG
-	char messageString[80];
-  	VisibilityStatus visibilityStatus;
-
-  	if ( printlevel == PL_HIGH )
-		visibilityStatus = VS_VISIBLE;
-	else
-		visibilityStatus = VS_HIDDEN;
-	#endif
-
-
-	/* III) UPDATE ACTIVE SET */
-	switch ( BC_status )
-	{
-		/* Optimal solution found as no working set change detected. */
-		case ST_UNDEFINED:
-			return RET_OPTIMAL_SOLUTION_FOUND;
-
-
-		/* Remove one variable from active set. */
-		case ST_INACTIVE:
-			#ifdef PC_DEBUG
-			sprintf( messageString,"bound no. %d.", BC_idx );
-			getGlobalMessageHandler( )->throwInfo( RET_REMOVE_FROM_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( removeBound( BC_idx,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_REMOVE_FROM_ACTIVESET_FAILED );
-
-			y[BC_idx] = 0.0;
-			break;
-
-
-		/* Add one variable to active set. */
-		default:
-			#ifdef PC_DEBUG
-			if ( BC_status == ST_LOWER )
-				sprintf( messageString,"lower bound no. %d.", BC_idx );
-			else
-				sprintf( messageString,"upper bound no. %d.", BC_idx );
-				getGlobalMessageHandler( )->throwInfo( RET_ADD_TO_ACTIVESET,messageString,__FUNCTION__,__FILE__,__LINE__,visibilityStatus );
-			#endif
-
-			if ( addBound( BC_idx,BC_status,BT_TRUE ) != SUCCESSFUL_RETURN )
-				return THROWERROR( RET_ADD_TO_ACTIVESET_FAILED );
-			break;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-
-/*
- *	p r i n t I t e r a t i o n
- */
-returnValue QProblemB::printIteration( 	int iteration,
-										int BC_idx,	SubjectToStatus BC_status
-		  								)
-{
-	char myPrintfString[160];
-
-	/* consistency check */
-	if ( iteration < 0 )
-		return THROWERROR( RET_INVALID_ARGUMENTS );
-
-	/* nothing to do */
-	if ( printlevel != PL_MEDIUM )
-		return SUCCESSFUL_RETURN;
-
-
-	/* 1) Print header at first iteration. */
- 	if ( iteration == 0 )
-	{
-		sprintf( myPrintfString,"\n##############  qpOASES  --  QP NO.%4.1d  ###############\n", count );
-		myPrintf( myPrintfString );
-
-		sprintf( myPrintfString,"   Iter   |   StepLength    |       Info      |   nFX   \n" );
-		myPrintf( myPrintfString );
-	}
-
-	/* 2) Print iteration line. */
-	if ( BC_status == ST_UNDEFINED )
-	{
-		sprintf( myPrintfString,"   %4.1d   |   %1.5e   |    QP SOLVED    |  %4.1d   \n", iteration,tau,getNFX( ) );
-		myPrintf( myPrintfString );
-	}
-	else
-	{
-		char info[8];
-
-		if ( BC_status == ST_INACTIVE )
-			sprintf( info,"REM BND" );
-		else
-			sprintf( info,"ADD BND" );
-
-		sprintf( myPrintfString,"   %4.1d   |   %1.5e   |   %s%4.1d   |  %4.1d   \n", iteration,tau,info,BC_idx,getNFX( ) );
-		myPrintf( myPrintfString );
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-#endif  /* PC_DEBUG */
-
-
-
-/*
- *	c h e c k K K T c o n d i t i o n s
- */
-returnValue QProblemB::checkKKTconditions( )
-{
-	#ifdef __PERFORM_KKT_TEST__
-
-	int i, j;
-	int nV = getNV( );
-
-	real_t tmp;
-	real_t maxKKTviolation = 0.0;
-
-
-	/* 1) Check for Hx + g - y*A' = 0  (here: A = Id). */
-	for( i=0; i<nV; ++i )
-	{
-		tmp = g[i];
-
-		for( j=0; j<nV; ++j )
-			tmp += H[i*nV + j] * x[j];
-
-		tmp -= y[i];
-
-		if ( getAbs( tmp ) > maxKKTviolation )
-			maxKKTviolation = getAbs( tmp );
-	}
-
-	/* 2) Check for lb <= x <= ub. */
-	for( i=0; i<nV; ++i )
-	{
-		if ( lb[i] - x[i] > maxKKTviolation )
-			maxKKTviolation = lb[i] - x[i];
-
-		if ( x[i] - ub[i] > maxKKTviolation )
-			maxKKTviolation = x[i] - ub[i];
-	}
-
-	/* 3) Check for correct sign of y and for complementary slackness. */
-	for( i=0; i<nV; ++i )
-	{
-		switch ( bounds.getStatus( i ) )
-		{
-			case ST_LOWER:
-				if ( -y[i] > maxKKTviolation )
-					maxKKTviolation = -y[i];
-				if ( getAbs( ( x[i] - lb[i] ) * y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( x[i] - lb[i] ) * y[i] );
-				break;
-
-			case ST_UPPER:
-				if ( y[i] > maxKKTviolation )
-					maxKKTviolation = y[i];
-				if ( getAbs( ( ub[i] - x[i] ) * y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( ( ub[i] - x[i] ) * y[i] );
-				break;
-
-			default: /* inactive */
-			if ( getAbs( y[i] ) > maxKKTviolation )
-					maxKKTviolation = getAbs( y[i] );
-				break;
-		}
-	}
-
-	if ( maxKKTviolation > CRITICALACCURACY )
-		return RET_NO_SOLUTION;
-
-	if ( maxKKTviolation > DESIREDACCURACY )
-		return RET_INACCURATE_SOLUTION;
-
-	#endif /* __PERFORM_KKT_TEST__ */
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/QProblemB.ipp b/phonelibs/qpoases/SRC/QProblemB.ipp
deleted file mode 100644
index 0b031301e..000000000
--- a/phonelibs/qpoases/SRC/QProblemB.ipp
+++ /dev/null
@@ -1,425 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/QProblemB.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of inlined member functions of the QProblemB class which 
- *	is able to use the newly developed online active set strategy for 
- *	parametric quadratic programming.
- */
-
-
-
-#include <math.h>
-
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-/*
- *	g e t H
- */
-inline returnValue QProblemB::getH( real_t* const _H ) const
-{
-	int i;
-
-	for ( i=0; i<getNV( )*getNV( ); ++i )
-		_H[i] = H[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t G
- */
-inline returnValue QProblemB::getG( real_t* const _g ) const
-{
-	int i;
-
-	for ( i=0; i<getNV( ); ++i )
-		_g[i] = g[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t L B
- */
-inline returnValue QProblemB::getLB( real_t* const _lb ) const
-{
-	int i;
-
-	for ( i=0; i<getNV( ); ++i )
-		_lb[i] = lb[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t L B
- */
-inline returnValue QProblemB::getLB( int number, real_t& value ) const
-{
-	if ( ( number >= 0 ) && ( number < getNV( ) ) )
-	{
-		value = lb[number];
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	g e t U B
- */
-inline returnValue QProblemB::getUB( real_t* const _ub ) const
-{
-	int i;
-
-	for ( i=0; i<getNV( ); ++i )
-		_ub[i] = ub[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t U B
- */
-inline returnValue QProblemB::getUB( int number, real_t& value ) const
-{
-	if ( ( number >= 0 ) && ( number < getNV( ) ) )
-	{
-		value = ub[number];
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	g e t B o u n d s
- */
-inline returnValue QProblemB::getBounds( Bounds* const _bounds ) const
-{
-	*_bounds = bounds;
-	
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	g e t N V
- */
-inline int QProblemB::getNV( ) const
-{
-	return bounds.getNV( );
-}
-
-
-/*
- *	g e t N F R
- */
-inline int QProblemB::getNFR( )
-{
-	return bounds.getNFR( );
-}
-
-
-/*
- *	g e t N F X
- */
-inline int QProblemB::getNFX( )
-{
-	return bounds.getNFX( );
-}
-
-
-/*
- *	g e t N F V
- */
-inline int QProblemB::getNFV( ) const
-{
-	return bounds.getNFV( );
-}
-
-
-/*
- *	g e t S t a t u s
- */
-inline QProblemStatus QProblemB::getStatus( ) const
-{
-	return status;
-}
-
-
-/*
- *	i s I n i t i a l i s e d
- */
-inline BooleanType QProblemB::isInitialised( ) const
-{
-	if ( status == QPS_NOTINITIALISED )
-		return BT_FALSE;
-	else
-		return BT_TRUE;
-}
-
-
-/*
- *	i s S o l v e d
- */
-inline BooleanType QProblemB::isSolved( ) const
-{
-	if ( status == QPS_SOLVED )
-		return BT_TRUE;
-	else
-		return BT_FALSE;
-}
-
-
-/*
- *	i s I n f e a s i b l e
- */
-inline BooleanType QProblemB::isInfeasible( ) const
-{
-	return infeasible;
-}
-
-
-/*
- *	i s U n b o u n d e d
- */
-inline BooleanType QProblemB::isUnbounded( ) const
-{
-	return unbounded;
-}
-
-
-/*
- *	g e t P r i n t L e v e l
- */
-inline PrintLevel QProblemB::getPrintLevel( ) const
-{
-	return printlevel;
-}
-
-
-/*
- *	g e t H e s s i a n T y p e
- */
-inline HessianType QProblemB::getHessianType( ) const
-{
-	return hessianType;
-}
-
-
-/*
- *	s e t H e s s i a n T y p e
- */
-inline returnValue QProblemB::setHessianType( HessianType _hessianType )
-{
-	hessianType = _hessianType;
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	s e t H
- */
-inline returnValue QProblemB::setH( const real_t* const H_new )
-{
-	int i, j;
-
-	int nV = getNV();
-
-	for( i=0; i<nV; ++i )
-		for( j=0; j<nV; ++j )
-			H[i*NVMAX + j] = H_new[i*nV + j];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t G
- */
-inline returnValue QProblemB::setG( const real_t* const g_new )
-{
-	int i;
-
-	int nV = getNV();
-
-	for( i=0; i<nV; ++i )
-		g[i] = g_new[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-inline returnValue QProblemB::setLB( const real_t* const lb_new )
-{
-	int i;
-
-	int nV = getNV();
-
-	for( i=0; i<nV; ++i )
-		lb[i] = lb_new[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t L B
- */
-inline returnValue QProblemB::setLB( int number, real_t value )
-{
-	if ( ( number >= 0 ) && ( number < getNV( ) ) )
-	{
-		lb[number] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	s e t U B
- */
-inline returnValue QProblemB::setUB( const real_t* const ub_new )
-{
-	int i;
-
-	int nV = getNV();
-
-	for( i=0; i<nV; ++i )
-		ub[i] = ub_new[i];
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s e t U B
- */
-inline returnValue QProblemB::setUB( int number, real_t value )
-{
-	if ( ( number >= 0 ) && ( number < getNV( ) ) )
-	{
-		ub[number] = value;
-
-		return SUCCESSFUL_RETURN;
-	}
-	else
-	{
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-	}
-}
-
-
-/*
- *	c o m p u t e G i v e n s
- */
-inline void QProblemB::computeGivens(	real_t xold, real_t yold, real_t& xnew, real_t& ynew,
-										real_t& c, real_t& s 
-										) const
-{
-    if ( getAbs( yold ) <= ZERO )
-	{
-        c = 1.0;
-        s = 0.0;
-		
-		xnew = xold;
-		ynew = yold;
-	}
-    else
-	{
-		real_t t, mu;
-
-        mu = getAbs( xold );
-		if ( getAbs( yold ) > mu )
-			mu = getAbs( yold );
-		
-        t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
-		
-		if ( xold < 0.0 )
-            t = -t;
-		
-        c = xold/t;
-        s = yold/t;
-        xnew = t;
-        ynew = 0.0;
-	}
-	
-	return;
-}
-
-		
-/*
- *	a p p l y G i v e n s
- */
-inline void QProblemB::applyGivens(	real_t c, real_t s, real_t xold, real_t yold,
-									real_t& xnew, real_t& ynew 
-									) const
-{
-	/* Usual Givens plane rotation requiring four multiplications. */
-	xnew =  c*xold + s*yold;
-	ynew = -s*xold + c*yold;
-// 	double nu = s/(1.0+c);
-// 
-// 	xnew = xold*c + yold*s;
-// 	ynew = (xnew+xold)*nu - yold;
-	
-	return;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/SubjectTo.cpp b/phonelibs/qpoases/SRC/SubjectTo.cpp
deleted file mode 100644
index 371f0d76d..000000000
--- a/phonelibs/qpoases/SRC/SubjectTo.cpp
+++ /dev/null
@@ -1,200 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/SubjectTo.cpp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the SubjectTo class designed to manage working sets of
- *	constraints and bounds within a QProblem.
- */
-
-
-#include <SubjectTo.hpp>
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
-
-
-/*
- *	S u b j e c t T o
- */
-SubjectTo::SubjectTo( ) :	noLower( BT_TRUE ),
-							noUpper( BT_TRUE ),
-							size( 0 )
-{
-	int i;
-
-	for( i=0; i<size; ++i )
-	{
-		type[i] = ST_UNKNOWN;
-		status[i] = ST_UNDEFINED;
-	}
-}
-
-
-/*
- *	S u b j e c t T o
- */
-SubjectTo::SubjectTo( const SubjectTo& rhs ) :	noLower( rhs.noLower ),
-												noUpper( rhs.noUpper ),
-												size( rhs.size )
-{
-	int i;
-
-	for( i=0; i<size; ++i )
-	{
-		type[i] = rhs.type[i];
-		status[i] = rhs.status[i];
-	}
-}
-
-
-/*
- *	~ S u b j e c t T o
- */
-SubjectTo::~SubjectTo( )
-{
-}
-
-
-/*
- *	o p e r a t o r =
- */
-SubjectTo& SubjectTo::operator=( const SubjectTo& rhs )
-{
-	int i;
-
-	if ( this != &rhs )
-	{
-		size = rhs.size;
-
-		for( i=0; i<size; ++i )
-		{
-			type[i] = rhs.type[i];
-			status[i] = rhs.status[i];
-		}
-
-		noLower = rhs.noLower;
-		noUpper = rhs.noUpper;
-	}
-
-	return *this;
-}
-
-
-
-/*
- *	i n i t
- */
-returnValue SubjectTo::init( int n )
-{
-	int i;
-
-	size = n;
-
-	noLower = BT_TRUE;
-	noUpper = BT_TRUE;
-
-	for( i=0; i<size; ++i )
-	{
-		type[i] = ST_UNKNOWN;
-		status[i] = ST_UNDEFINED;
-	}
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-
-/*****************************************************************************
- *  P R O T E C T E D                                                        *
- *****************************************************************************/
-
-/*
- *	a d d I n d e x
- */
-returnValue SubjectTo::addIndex(	Indexlist* const indexlist,
-									int newnumber, SubjectToStatus newstatus
-									)
-{
-	/* consistency check */
-	if ( status[newnumber] == newstatus )
-		return THROWERROR( RET_INDEX_ALREADY_OF_DESIRED_STATUS );
-
-	status[newnumber] = newstatus;
-
-	if ( indexlist->addNumber( newnumber ) == RET_INDEXLIST_EXCEEDS_MAX_LENGTH )
-		return THROWERROR( RET_ADDINDEX_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e m o v e I n d e x
- */
-returnValue SubjectTo::removeIndex(	Indexlist* const indexlist, 
-									int removenumber
-									)
-{
-	status[removenumber] = ST_UNDEFINED;
-
-	if ( indexlist->removeNumber( removenumber ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_UNKNOWN_BUG );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	s w a p I n d e x
- */
-returnValue SubjectTo::swapIndex(	Indexlist* const indexlist,
-									int number1, int number2
-									)
-{
-	/* consistency checks */
-	if ( status[number1] != status[number2] )
-		return THROWERROR( RET_SWAPINDEX_FAILED );
-
-	if ( number1 == number2 )
-	{
-		THROWWARNING( RET_NOTHING_TO_DO );
-		return SUCCESSFUL_RETURN;
-	}
-
-	if ( indexlist->swapNumbers( number1,number2 ) != SUCCESSFUL_RETURN )
-		return THROWERROR( RET_SWAPINDEX_FAILED );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/SubjectTo.ipp b/phonelibs/qpoases/SRC/SubjectTo.ipp
deleted file mode 100644
index 32215ba7e..000000000
--- a/phonelibs/qpoases/SRC/SubjectTo.ipp
+++ /dev/null
@@ -1,132 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/SubjectTo.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of the inlined member functions of the SubjectTo class 
- *	designed to manage working sets of constraints and bounds within a QProblem.
- */
-
-
-/*****************************************************************************
- *  P U B L I C                                                              *
- *****************************************************************************/
- 
-
-/*
- *	g e t T y p e
- */
-inline SubjectToType SubjectTo::getType( int i ) const
-{
-	if ( ( i >= 0 ) && ( i < size ) )
-		return type[i];
-	else
-		return ST_UNKNOWN;
-}
-
-
-/*
- *	g e t S t a t u s
- */
-inline SubjectToStatus SubjectTo::getStatus( int i ) const
-{
-	if ( ( i >= 0 ) && ( i < size ) )
-		return status[i];
-	else
-		return ST_UNDEFINED;
-}
-
-
-/*
- *	s e t T y p e
- */
-inline returnValue SubjectTo::setType( int i, SubjectToType value )
-{
-	if ( ( i >= 0 ) && ( i < size ) )
-	{
-		type[i] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t S t a t u s
- */
-inline returnValue SubjectTo::setStatus( int i, SubjectToStatus value )
-{
-	if ( ( i >= 0 ) && ( i < size ) )
-	{
-		status[i] = value;
-		return SUCCESSFUL_RETURN;
-	}
-	else
-		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
-}
-
-
-/*
- *	s e t N o L o w e r
- */
-inline void SubjectTo::setNoLower( BooleanType _status )
-{
-	noLower = _status;
-}
- 
-
-/*
- *	s e t N o U p p e r
- */
-inline void SubjectTo::setNoUpper( BooleanType _status )
-{
-	noUpper = _status;
-}
-
-
-/*
- *	i s N o L o w e r
- */
-inline BooleanType SubjectTo::isNoLower( ) const
-{
-	return noLower;
-}
-
- 
-/*
- *	i s N o L o w e r
- */
-inline BooleanType SubjectTo::isNoUpper( ) const
-{
-	return noUpper;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Utils.cpp b/phonelibs/qpoases/SRC/Utils.cpp
deleted file mode 100644
index c11722c47..000000000
--- a/phonelibs/qpoases/SRC/Utils.cpp
+++ /dev/null
@@ -1,471 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-/**
- *	\file SRC/Utils.cpp
- *	\author Hans Joachim Ferreau, Eckhard Arnold
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of some inlined utilities for working with the different QProblem
- *  classes.
- */
-
-
-#include <math.h>
-
-#if defined(__WIN32__) || defined(WIN32)
-  #include <windows.h>
-#elif defined(LINUX)
-  #include <sys/stat.h>
-  #include <sys/time.h>
-#endif
-
-#ifdef __MATLAB__
-  #include <mex.h>
-#endif
-
-
-#include <Utils.hpp>
-
-
-
-#ifdef PC_DEBUG  /* Define print functions only for debugging! */
-/*
- *	p r i n t
- */
-returnValue print( const real_t* const v, int n )
-{
-	int i;
-	char myPrintfString[160];
-
-	/* Print a vector. */
-	myPrintf( "[\t" );
-	for( i=0; i<n; ++i )
-	{
-		sprintf( myPrintfString," %.16e\t", v[i] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "]\n" );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const v, int n,
-					const int* const V_idx
-					)
-{
-	int i;
-	char myPrintfString[160];
-
-	/* Print a permuted vector. */
-	myPrintf( "[\t" );
-	for( i=0; i<n; ++i )
-	{
-		sprintf( myPrintfString," %.16e\t", v[ V_idx[i] ] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "]\n" );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const v, int n,
-					const char* name
-					)
-{
-	char myPrintfString[160];
-
-	/* Print vector name ... */
-	sprintf( myPrintfString,"%s = ", name );
-	myPrintf( myPrintfString );
-
-	/* ... and the vector itself. */
-	return print( v, n );
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print( const real_t* const M, int nrow, int ncol )
-{
-	int i;
-
-	/* Print a matrix as a collection of row vectors. */
-	for( i=0; i<nrow; ++i )
-		print( &(M[i*ncol]), ncol );
-	myPrintf( "\n" );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const M, int nrow, int ncol,
-					const int* const ROW_idx, const int* const COL_idx
-					)
-{
-	int i;
-
-	/* Print a permuted matrix as a collection of permuted row vectors. */
-	for( i=0; i<nrow; ++i )
-		print( &( M[ ROW_idx[i]*ncol ] ), ncol, COL_idx );
-	myPrintf( "\n" );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const real_t* const M, int nrow, int ncol,
-					const char* name
-					)
-{
-	char myPrintfString[160];
-
-	/* Print matrix name ... */
-	sprintf( myPrintfString,"%s = ", name );
-	myPrintf( myPrintfString );
-
-	/* ... and the matrix itself. */
-	return print( M, nrow, ncol );
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print( const int* const index, int n )
-{
-	int i;
-	char myPrintfString[160];
-
-	/* Print a indexlist. */
-	myPrintf( "[\t" );
-	for( i=0; i<n; ++i )
-	{
-		sprintf( myPrintfString," %d\t", index[i] );
-		myPrintf( myPrintfString );
-	}
-	myPrintf( "]\n" );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t
- */
-returnValue print(	const int* const index, int n,
-					const char* name
-					)
-{
-	char myPrintfString[160];
-
-	/* Print indexlist name ... */
-	sprintf( myPrintfString,"%s = ", name );
-	myPrintf( myPrintfString );
-
-	/* ... and the indexlist itself. */
-	return print( index, n );
-}
-
-
-/*
- *	m y P r i n t f
- */
-returnValue myPrintf( const char* s )
-{
-	#ifdef __MATLAB__
-	mexPrintf( s );
-	#else
-	myFILE* outputfile = getGlobalMessageHandler( )->getOutputFile( );
-	if ( outputfile == 0 )
-		return THROWERROR( RET_NO_GLOBAL_MESSAGE_OUTPUTFILE );
-
-	fprintf( outputfile, "%s", s );
-	#endif
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	p r i n t C o p y r i g h t N o t i c e
- */
-returnValue printCopyrightNotice( )
-{
-	return myPrintf( "\nqpOASES -- An Implementation of the Online Active Set Strategy.\nCopyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.\n\nqpOASES is distributed under the terms of the \nGNU Lesser General Public License 2.1 in the hope that it will be \nuseful, but WITHOUT ANY WARRANTY; without even the implied warranty \nof MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. \nSee the GNU Lesser General Public License for more details.\n\n" );
-}
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	real_t* data, int nrow, int ncol,
-							const char* datafilename
-							)
-{
-	int i, j;
-	float float_data;
-	myFILE* datafile;
-
-	/* 1) Open file. */
-	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
-	{
-		char errstr[80];
-		sprintf( errstr,"(%s)",datafilename );
-		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	}
-
-	/* 2) Read data from file. */
-	for( i=0; i<nrow; ++i )
-	{
-		for( j=0; j<ncol; ++j )
-		{
-			if ( fscanf( datafile, "%f ", &float_data ) == 0 )
-			{
-				fclose( datafile );
-				char errstr[80];
-				sprintf( errstr,"(%s)",datafilename );
-				return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-			}
-			data[i*ncol + j] = ( (real_t) float_data );
-		}
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	real_t* data, int n,
-							const char* datafilename
-							)
-{
-	return readFromFile( data, n, 1, datafilename );
-}
-
-
-
-/*
- *	r e a d F r o m F i l e
- */
-returnValue readFromFile(	int* data, int n,
-							const char* datafilename
-							)
-{
-	int i;
-	myFILE* datafile;
-
-	/* 1) Open file. */
-	if ( ( datafile = fopen( datafilename, "r" ) ) == 0 )
-	{
-		char errstr[80];
-		sprintf( errstr,"(%s)",datafilename );
-		return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-	}
-
-	/* 2) Read data from file. */
-	for( i=0; i<n; ++i )
-	{
-		if ( fscanf( datafile, "%d\n", &(data[i]) ) == 0 )
-		{
-			fclose( datafile );
-			char errstr[80];
-			sprintf( errstr,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_READ_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const real_t* const data, int nrow, int ncol,
-							const char* datafilename, BooleanType append
-							)
-{
-	int i, j;
-	myFILE* datafile;
-
-	/* 1) Open file. */
-	if ( append == BT_TRUE )
-	{
-		/* append data */
-		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
-		{
-			char errstr[80];
-			sprintf( errstr,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-	else
-	{
-		/* do not append data */
-		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
-		{
-			char errstr[80];
-			sprintf( errstr,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 2) Write data into file. */
-	for( i=0; i<nrow; ++i )
-	{
-		for( j=0; j<ncol; ++j )
-		 	fprintf( datafile, "%.16e ", data[i*ncol+j] );
-
-		fprintf( datafile, "\n" );
-	}
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const real_t* const data, int n,
-							const char* datafilename, BooleanType append
-							)
-{
-	return writeIntoFile( data,1,n,datafilename,append );
-}
-
-
-/*
- *	w r i t e I n t o F i l e
- */
-returnValue writeIntoFile(	const int* const data, int n,
-							const char* datafilename, BooleanType append
-							)
-{
-	int i;
-
-	myFILE* datafile;
-
-	/* 1) Open file. */
-	if ( append == BT_TRUE )
-	{
-		/* append data */
-		if ( ( datafile = fopen( datafilename, "a" ) ) == 0 )
-		{
-			char errstr[80];
-			sprintf( errstr,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-	else
-	{
-		/* do not append data */
-		if ( ( datafile = fopen( datafilename, "w" ) ) == 0 )
-		{
-			char errstr[80];
-			sprintf( errstr,"(%s)",datafilename );
-			return getGlobalMessageHandler( )->throwError( RET_UNABLE_TO_OPEN_FILE,errstr,__FUNCTION__,__FILE__,__LINE__,VS_VISIBLE );
-		}
-	}
-
-	/* 2) Write data into file. */
-	for( i=0; i<n; ++i )
-		fprintf( datafile, "%d\n", data[i] );
-
-	/* 3) Close file. */
-	fclose( datafile );
-
-	return SUCCESSFUL_RETURN;
-}
-#endif  /* PC_DEBUG */
-
-
-/*
- *	g e t C P U t i m e
- */
-real_t getCPUtime( )
-{
-	real_t current_time = -1.0;
-
-	#if defined(__WIN32__) || defined(WIN32)
-	LARGE_INTEGER counter, frequency;
-	QueryPerformanceFrequency(&frequency);
-	QueryPerformanceCounter(&counter);
-	current_time = ((real_t) counter.QuadPart) / ((real_t) frequency.QuadPart);
-	#elif defined(LINUX)
-	struct timeval theclock;
-	gettimeofday( &theclock,0 );
-	current_time = 1.0*theclock.tv_sec + 1.0e-6*theclock.tv_usec;
-	#endif
-
-	return current_time;
-}
-
-
-/*
- *	g e t N o r m
- */
-real_t getNorm( const real_t* const v, int n )
-{
-	int i;
-
-	real_t norm = 0.0;
-
-	for( i=0; i<n; ++i )
-		norm += v[i]*v[i];
-
-	return sqrt( norm );
-}
-
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/SRC/Utils.ipp b/phonelibs/qpoases/SRC/Utils.ipp
deleted file mode 100644
index 861bdb372..000000000
--- a/phonelibs/qpoases/SRC/Utils.ipp
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- *	This file is part of qpOASES.
- *
- *	qpOASES -- An Implementation of the Online Active Set Strategy.
- *	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
- *
- *	qpOASES is free software; you can redistribute it and/or
- *	modify it under the terms of the GNU Lesser General Public
- *	License as published by the Free Software Foundation; either
- *	version 2.1 of the License, or (at your option) any later version.
- *
- *	qpOASES is distributed in the hope that it will be useful,
- *	but WITHOUT ANY WARRANTY; without even the implied warranty of
- *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
- *	Lesser General Public License for more details.
- *
- *	You should have received a copy of the GNU Lesser General Public
- *	License along with qpOASES; if not, write to the Free Software
- *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
- *
- */
-
-
-
-/**
- *	\file SRC/Utils.ipp
- *	\author Hans Joachim Ferreau
- *	\version 1.3embedded
- *	\date 2007-2008
- *
- *	Implementation of some inlined utilities for working with the different QProblem 
- *  classes.
- */
-
-
-
-/*
- *	g e t A b s
- */
-inline real_t getAbs( real_t x )
-{
-	if ( x < 0.0 )
-		return -x;
-	else
-		return x;
-}
-
-
-/*
- *	end of file
- */
diff --git a/phonelibs/qpoases/VERSIONS.txt b/phonelibs/qpoases/VERSIONS.txt
deleted file mode 100644
index 35a6740b1..000000000
--- a/phonelibs/qpoases/VERSIONS.txt
+++ /dev/null
@@ -1,87 +0,0 @@
-##
-##	qpOASES -- An Implementation of the Online Active Set Strategy.
-##	Copyright (C) 2007-2008 by Hans Joachim Ferreau et al. All rights reserved.
-##
-##	qpOASES is free software; you can redistribute it and/or
-##	modify it under the terms of the GNU Lesser General Public
-##	License as published by the Free Software Foundation; either
-##	version 2.1 of the License, or (at your option) any later version.
-##
-##	qpOASES is distributed in the hope that it will be useful,
-##	but WITHOUT ANY WARRANTY; without even the implied warranty of
-##	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
-##	Lesser General Public License for more details.
-##
-##	You should have received a copy of the GNU Lesser General Public
-##	License along with qpOASES; if not, write to the Free Software
-##	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
-##
-
-
-
-VERSION HISTORY
-===============
-
-1.3embedded (last updated on 30th April 2009):
------------------------------------------------------------------------
-
-+ Re-programming of internal memory management to avoid dynamic memory allocations 
-+ Most #ifdef directives removed
-+ Almost all type definitions gathered within INCLUDE/Types.hpp
-+ Irrelevant functionality removed (like the SQProblem class, functionality 
-  for loading data from files or the SCILAB interface)
-+ Replacement of all doubles by real_t
-+ Introduction of define "PC_DEBUG" for switching off all print functions
-+ stdio.h was made optional, string.h is no longer needed
-+ relative paths removed from #include directives
-+ made auxiliary objects locally static within solveInitialQP()
-+ Matlab interface fixed for single precision
-+ New return value -2 from Legacy wrapper added to Matlab/Simulink interfaces
-+ KKT optimality check moved into QProblem(B) class, SolutionAnalysis class removed
-
-
-1.3 (released on 2nd June 2008, last updated on 19th June 2008):
------------------------------------------------------------------------
-
-+ Implementation of "initialised homotopy" concept
-+ Addition of the SolutionAnalysis class
-+ Utility functions for solving test problems in OQP format added
-+ Flexibility of Matlab(R) interface enhanced
-+ Major source code cleanup
-  (Attention: a few class names and calling interfaces have changed!)
-
-
-
-1.2 (released on 9th October 2007):
------------------------------------------------------------------------
-
-+ Special treatment of diagonal Hessians
-+ Improved infeasibility detection
-+ Further improved Matlab(R) interface
-+ Extended Simulink(R) interface
-+ scilab interface added
-+ Code cleanup and several bugfixes
-
-
-
-1.1 (released on 8th July 2007):
---------------------------------
-
-+ Implementation of the QProblemB class
-+ Basic implementation of the SQProblem class
-+ Improved Matlab(R) interface
-+ Enabling/Disabling of constraints introduced
-+ Several bugfixes
-
-
-
-1.0 (released on 17th April 2007):
-----------------------------------
-
-Initial release.
-
-
-
-##
-##	end of file
-##
diff --git a/pyextra/acados_template/__init__.py b/pyextra/acados_template/__init__.py
new file mode 100644
index 000000000..f33b75bb7
--- /dev/null
+++ b/pyextra/acados_template/__init__.py
@@ -0,0 +1,43 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+from .acados_model import *
+from .generate_c_code_explicit_ode import *
+from .generate_c_code_implicit_ode import *
+from .generate_c_code_constraint import *
+from .generate_c_code_nls_cost import *
+from .acados_ocp import *
+from .acados_sim import *
+from .acados_ocp_solver import *
+from .acados_sim_solver import *
+from .utils import *
diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json
new file mode 100644
index 000000000..f4e332351
--- /dev/null
+++ b/pyextra/acados_template/acados_layout.json
@@ -0,0 +1,775 @@
+{
+    "code_export_directory": [
+        "str"
+    ],
+    "acados_include_path": [
+        "str"
+    ],
+    "model": {
+         "name" : [
+            "str"
+        ],
+        "dyn_ext_fun_type" : [
+            "str"
+        ],
+        "dyn_source_discrete" : [
+            "str"
+        ],
+        "dyn_disc_fun_jac_hess" : [
+            "str"
+        ],
+        "dyn_disc_fun_jac" : [
+            "str"
+        ],
+        "dyn_disc_fun" : [
+            "str"
+        ]
+    },
+    "parameter_values": [
+        "ndarray",
+        [
+            "np"
+        ]
+    ],
+    "acados_lib_path": [
+        "str"
+    ],
+    "problem_class": [
+        "str"
+    ],
+    "constraints": {
+        "constr_type": [
+            "str"
+        ],
+        "constr_type_e": [
+            "str"
+        ],
+        "lbx": [
+            "ndarray",
+                [
+                    "nbx"
+                ]
+        ],
+        "lbu": [
+            "ndarray",
+            [
+                "nbu"
+            ]
+        ],
+        "ubx": [
+            "ndarray",
+                [
+                    "nbx"
+                ]
+        ],
+        "ubu": [
+            "ndarray",
+            [
+                "nbu"
+            ]
+        ],
+        "idxbx": [
+            "ndarray",
+                [
+                    "nbx"
+                ]
+        ],
+        "idxbu": [
+            "ndarray",
+            [
+                "nbu"
+            ]
+        ],
+        "lbx_e": [
+            "ndarray",
+            [
+                "nbx_e"
+            ]
+        ],
+        "ubx_e": [
+            "ndarray",
+            [
+                "nbx_e"
+            ]
+        ],
+        "idxbx_e": [
+            "ndarray",
+            [
+                "nbx_e"
+            ]
+        ],
+        "lbx_0": [
+            "ndarray",
+            [
+                "nbx_0"
+            ]
+        ],
+        "ubx_0": [
+            "ndarray",
+            [
+                "nbx_0"
+            ]
+        ],
+        "idxbx_0": [
+            "ndarray",
+            [
+                "nbx_0"
+            ]
+        ],
+        "idxbxe_0": [
+            "ndarray",
+            [
+                "nbxe_0"
+            ]
+        ],
+        "lg": [
+            "ndarray",
+            [
+                "ng"
+            ]
+        ],
+        "ug": [
+            "ndarray",
+            [
+                "ng"
+            ]
+        ],
+        "D": [
+            "ndarray",
+                [
+                    "ng",
+                    "nu"
+                ]
+        ],
+        "C": [
+            "ndarray",
+            [
+                "ng",
+                "nx"
+            ]
+        ],
+        "C_e": [
+            "ndarray",
+            [
+                "ng_e",
+                "nx"
+            ]
+        ],
+        "lg_e": [
+            "ndarray",
+            [
+                "ng_e"
+            ]
+        ],
+        "ug_e": [
+            "ndarray",
+            [
+                "ng_e"
+            ]
+        ],
+        "lh": [
+            "ndarray",
+            [
+                "nh"
+            ]
+        ],
+        "uh": [
+            "ndarray",
+            [
+                "nh"
+            ]
+        ],
+        "lh_e": [
+            "ndarray",
+            [
+                "nh_e"
+            ]
+        ],
+        "uh_e": [
+            "ndarray",
+            [
+                "nh_e"
+            ]
+        ],
+        "lphi": [
+            "ndarray",
+            [
+                "nphi"
+            ]
+        ],
+        "uphi": [
+            "ndarray",
+            [
+                "nphi"
+            ]
+        ],
+        "lphi_e": [
+            "ndarray",
+            [
+                "nphi_e"
+            ]
+        ],
+        "uphi_e": [
+            "ndarray",
+            [
+                "nphi_e"
+            ]
+        ],
+        "lsbx": [
+            "ndarray",
+                [
+                    "nsbx"
+                ]
+        ],
+        "usbx": [
+            "ndarray",
+                [
+                    "nsbx"
+                ]
+        ],
+        "lsbu": [
+            "ndarray",
+            [
+                "nsbu"
+            ]
+        ],
+        "usbu": [
+            "ndarray",
+            [
+                "nsbu"
+            ]
+        ],
+        "idxsbx": [
+            "ndarray",
+                [
+                    "nsbx"
+                ]
+        ],
+        "idxsbu": [
+            "ndarray",
+            [
+                "nsbu"
+            ]
+        ],
+        "lsbx_e": [
+            "ndarray",
+                [
+                    "nsbx_e"
+                ]
+        ],
+        "usbx_e": [
+            "ndarray",
+                [
+                    "nsbx_e"
+                ]
+        ],
+        "idxsbx_e": [
+            "ndarray",
+                [
+                    "nsbx_e"
+                ]
+        ],
+        "lsg": [
+            "ndarray",
+            [
+                "nsg"
+            ]
+        ],
+        "usg": [
+            "ndarray",
+            [
+                "nsg"
+            ]
+        ],
+        "idxsg": [
+            "ndarray",
+            [
+                "nsg"
+            ]
+        ],
+        "lsg_e": [
+            "ndarray",
+            [
+                "nsg_e"
+            ]
+        ],
+        "usg_e": [
+            "ndarray",
+            [
+                "nsg_e"
+            ]
+        ],
+        "idxsg_e": [
+            "ndarray",
+            [
+                "nsg_e"
+            ]
+        ],
+        "lsh": [
+            "ndarray",
+            [
+                "nsh"
+            ]
+        ],
+        "ush": [
+            "ndarray",
+            [
+                "nsh"
+            ]
+        ],
+        "idxsh": [
+            "ndarray",
+            [
+                "nsh"
+            ]
+        ],
+        "lsh_e": [
+            "ndarray",
+            [
+                "nsh_e"
+            ]
+        ],
+        "ush_e": [
+            "ndarray",
+            [
+                "nsh_e"
+            ]
+        ],
+        "idxsh_e": [
+            "ndarray",
+            [
+                "nsh_e"
+            ]
+        ],
+        "lsphi": [
+            "ndarray",
+            [
+                "nsphi"
+            ]
+        ],
+        "usphi": [
+            "ndarray",
+            [
+                "nsphi"
+            ]
+        ],
+        "idxsphi": [
+            "ndarray",
+            [
+                "nsphi"
+            ]
+        ],
+        "lsphi_e": [
+            "ndarray",
+            [
+                "nsphi_e"
+            ]
+        ],
+        "usphi_e": [
+            "ndarray",
+            [
+                "nsphi_e"
+            ]
+        ],
+        "idxsphi_e": [
+            "ndarray",
+            [
+                "nsphi_e"
+            ]
+        ]
+    },
+    "cost": {
+        "cost_type_0": [
+            "str"
+        ],
+        "cost_type": [
+            "str"
+        ],
+        "cost_type_e": [
+            "str"
+        ],
+        "cost_ext_fun_type_0": [
+            "str"
+        ],
+        "cost_ext_fun_type": [
+            "str"
+        ],
+        "cost_ext_fun_type_e": [
+            "str"
+        ],
+        "Vu_0": [
+            "ndarray",
+            [
+                "ny_0",
+                "nu"
+            ]
+        ],
+        "Vu": [
+            "ndarray",
+            [
+                "ny",
+                "nu"
+            ]
+        ],
+        "Vx_0": [
+            "ndarray",
+            [
+                "ny_0",
+                "nx"
+            ]
+        ],
+        "Vx": [
+            "ndarray",
+            [
+                "ny",
+                "nx"
+            ]
+        ],
+        "Vx_e": [
+            "ndarray",
+            [
+                "ny_e",
+                "nx"
+            ]
+        ],
+        "Vz_0": [
+            "ndarray",
+            [
+                "ny_0",
+                "nz"
+            ]
+        ],
+        "Vz": [
+            "ndarray",
+            [
+                "ny",
+                "nz"
+            ]
+        ],
+        "W_0": [
+            "ndarray",
+            [
+                "ny_0",
+                "ny_0"
+            ]
+        ],
+        "W": [
+            "ndarray",
+            [
+                "ny",
+                "ny"
+            ]
+        ],
+        "Zl": [
+            "ndarray",
+            [
+                "ns"
+            ]
+        ],
+        "Zu": [
+            "ndarray",
+            [
+                "ns"
+            ]
+        ],
+        "zl": [
+            "ndarray",
+            [
+                "ns"
+            ]
+        ],
+        "zu": [
+            "ndarray",
+            [
+                "ns"
+            ]
+        ],
+        "W_e": [
+            "ndarray",
+            [
+                "ny_e",
+                "ny_e"
+            ]
+        ],
+        "yref_0": [
+            "ndarray",
+            [
+                "ny_0"
+            ]
+        ],
+        "yref": [
+            "ndarray",
+            [
+                "ny"
+            ]
+        ],
+        "yref_e": [
+            "ndarray",
+            [
+                "ny_e"
+            ]
+        ],
+        "Zl_e": [
+            "ndarray",
+            [
+                "ns_e"
+            ]
+        ],
+        "Zu_e": [
+            "ndarray",
+            [
+                "ns_e"
+            ]
+        ],
+        "zl_e": [
+            "ndarray",
+            [
+                "ns_e"
+            ]
+        ],
+        "zu_e": [
+            "ndarray",
+            [
+                "ns_e"
+            ]
+        ]
+    },
+    "dims": {
+        "N": [
+            "int"
+        ],
+        "nbu": [
+            "int"
+        ],
+        "nbx": [
+            "int"
+        ],
+        "nsbu": [
+            "int"
+        ],
+        "nsbx": [
+            "int"
+        ],
+        "nsbx_e": [
+            "int"
+        ],
+        "nbx_0": [
+            "int"
+        ],
+        "nbx_e": [
+            "int"
+        ],
+        "nbxe_0": [
+            "int"
+        ],
+        "nsg": [
+            "int"
+        ],
+        "nsg_e": [
+            "int"
+        ],
+        "nsh": [
+            "int"
+        ],
+        "nsh_e": [
+            "int"
+        ],
+        "nsphi": [
+            "int"
+        ],
+        "nsphi_e": [
+            "int"
+        ],
+        "ns": [
+            "int"
+        ],
+        "ns_e": [
+            "int"
+        ],
+        "ng": [
+            "int"
+        ],
+        "ng_e": [
+            "int"
+        ],
+        "np": [
+            "int"
+        ],
+        "nr": [
+            "int"
+        ],
+        "nr_e": [
+            "int"
+        ],
+        "nh": [
+            "int"
+        ],
+        "nh_e": [
+            "int"
+        ],
+        "nphi": [
+            "int"
+        ],
+        "nphi_e": [
+            "int"
+        ],
+        "nu": [
+            "int"
+        ],
+        "nx": [
+            "int"
+        ],
+        "ny": [
+            "int"
+        ],
+        "ny_0": [
+            "int"
+        ],
+        "ny_e": [
+            "int"
+        ],
+        "nz": [
+            "int"
+        ],
+        "gnsf_nx1": [
+            "int"
+        ],
+        "gnsf_nz1": [
+            "int"
+        ],
+        "gnsf_nuhat": [
+            "int"
+        ],
+        "gnsf_ny": [
+            "int"
+        ],
+        "gnsf_nout": [
+            "int"
+        ]
+    },
+    "solver_options": {
+        "time_steps": [
+            "ndarray",
+            [
+                "N"
+            ]
+        ],
+        "hessian_approx": [
+            "str"
+        ],
+        "regularize_method": [
+            "str"
+        ],
+        "integrator_type": [
+            "str"
+        ],
+        "nlp_solver_type": [
+            "str"
+        ],
+        "globalization": [
+            "str"
+        ],
+        "nlp_solver_step_length": [
+            "float"
+        ],
+        "levenberg_marquardt": [
+            "float"
+        ],
+        "qp_solver": [
+            "str"
+        ],
+        "tf": [
+            "float"
+        ],
+        "Tsim": [
+            "float"
+        ],
+        "alpha_min": [
+            "float"
+        ],
+        "alpha_reduction": [
+            "float"
+        ],
+        "sim_method_num_stages": [
+            "ndarray",
+            [
+                "N"
+            ]
+        ],
+        "sim_method_num_steps": [
+            "ndarray",
+            [
+                "N"
+            ]
+        ],
+        "sim_method_newton_iter": [
+            "int"
+        ],
+        "sim_method_jac_reuse": [
+            "ndarray",
+            [
+                "N"
+            ]
+        ],
+        "qp_solver_cond_N": [
+            "int"
+        ],
+        "qp_solver_warm_start": [
+            "int"
+        ],
+        "qp_solver_tol_stat": [
+            "float"
+        ],
+        "qp_solver_tol_eq": [
+            "float"
+        ],
+        "qp_solver_tol_ineq": [
+            "float"
+        ],
+        "qp_solver_tol_comp": [
+            "float"
+        ],
+        "qp_solver_iter_max": [
+            "int"
+        ],
+        "nlp_solver_tol_stat": [
+            "float"
+        ],
+        "nlp_solver_tol_eq": [
+            "float"
+        ],
+        "nlp_solver_tol_ineq": [
+            "float"
+        ],
+        "nlp_solver_tol_comp": [
+            "float"
+        ],
+        "nlp_solver_max_iter": [
+            "int"
+        ],
+        "print_level": [
+            "int"
+        ],
+        "initialize_t_slacks": [
+            "int"
+        ],
+        "exact_hess_cost": [
+            "int"
+        ],
+        "exact_hess_constr": [
+            "int"
+        ],
+        "exact_hess_dyn": [
+            "int"
+        ],
+        "ext_cost_num_hess": [
+            "int"
+        ],
+        "model_external_shared_lib_dir": [
+            "str"
+        ],
+        "model_external_shared_lib_name": [
+            "str"
+        ]
+    }
+}
diff --git a/pyextra/acados_template/acados_model.py b/pyextra/acados_template/acados_model.py
new file mode 100644
index 000000000..379ade0d9
--- /dev/null
+++ b/pyextra/acados_template/acados_model.py
@@ -0,0 +1,151 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+class AcadosModel():
+    """
+    Class containing all the information to code generate the external CasADi functions
+    that are needed when creating an acados ocp solver or acados integrator.
+    Thus, this class contains:
+    
+    a) the :py:attr:`name` of the model,
+    b) all CasADi variables/expressions needed in the CasADi function generation process.
+    """
+    def __init__(self):
+        ## common for OCP and Integrator
+        self.name = None
+        """
+        The model name is used for code generation. Type: string. Default: :code:`None`
+        """
+        self.x = None           #: CasADi variable describing the state of the system; Default: :code:`None`
+        self.xdot = None        #: CasADi variable describing the derivative of the state wrt time; Default: :code:`None`
+        self.u = None           #: CasADi variable describing the input of the system; Default: :code:`None`
+        self.z = []             #: CasADi variable describing the algebraic variables of the DAE; Default: :code:`empty`
+        self.p = []             #: CasADi variable describing parameters of the DAE; Default: :code:`empty`
+        # dynamics
+        self.f_impl_expr = None
+        """
+        CasADi expression for the implicit dynamics :math:`f_\\text{impl}(\dot{x}, x, u, z, p) = 0`.
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'IRK'.
+        Default: :code:`None`
+        """
+        self.f_expl_expr = None
+        """
+        CasADi expression for the explicit dynamics :math:`\dot{x} = f_\\text{expl}(x, u, p)`.
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'ERK'.
+        Default: :code:`None`
+        """
+        self.disc_dyn_expr = None
+        """
+        CasADi expression for the discrete dynamics :math:`x_{+} = f_\\text{disc}(x, u, p)`.
+        Used if :py:attr:`acados_template.acados_ocp.AcadosOcpOptions.integrator_type` == 'DISCRETE'.
+        Default: :code:`None`
+        """
+
+        self.dyn_ext_fun_type = 'casadi'  #: type of external functions for dynamics module; 'casadi' or 'generic'; Default: 'casadi'
+        self.dyn_source_discrete = None  #: name of source file for discrete dyanamics; Default: :code:`None`
+        self.dyn_disc_fun_jac_hess = None  #: name of function discrete dyanamics + jacobian and hessian; Default: :code:`None`
+        self.dyn_disc_fun_jac = None  #: name of function discrete dyanamics + jacobian; Default: :code:`None`
+        self.dyn_disc_fun = None  #: name of function discrete dyanamics; Default: :code:`None`
+
+        ## for OCP
+        # constraints
+        self.con_h_expr = None  #: CasADi expression for the constraint :math:`h`; Default: :code:`None`
+        self.con_phi_expr = None  #: CasADi expression for the constraint phi; Default: :code:`None`
+        self.con_r_expr = None  #: CasADi expression for the constraint phi(r); Default: :code:`None`
+        self.con_r_in_phi = None
+        # terminal
+        self.con_h_expr_e = None  #: CasADi expression for the terminal constraint :math:`h^e`; Default: :code:`None`
+        self.con_r_expr_e = None  #: CasADi expression for the terminal constraint; Default: :code:`None`
+        self.con_phi_expr_e = None  #: CasADi expression for the terminal constraint; Default: :code:`None`
+        self.con_r_in_phi_e = None
+        # cost
+        self.cost_y_expr = None  #: CasADi expression for nonlinear least squares; Default: :code:`None`
+        self.cost_y_expr_e = None  #: CasADi expression for nonlinear least squares, terminal; Default: :code:`None`
+        self.cost_y_expr_0 = None  #: CasADi expression for nonlinear least squares, initial; Default: :code:`None`
+        self.cost_expr_ext_cost = None  #: CasADi expression for external cost; Default: :code:`None`
+        self.cost_expr_ext_cost_e = None  #: CasADi expression for external cost, terminal; Default: :code:`None`
+        self.cost_expr_ext_cost_0 = None  #: CasADi expression for external cost, initial; Default: :code:`None`
+
+
+def acados_model_strip_casadi_symbolics(model):
+    out = model
+    if 'f_impl_expr' in out.keys():
+        del out['f_impl_expr']
+    if 'f_expl_expr' in out.keys():
+        del out['f_expl_expr']
+    if 'disc_dyn_expr' in out.keys():
+        del out['disc_dyn_expr']
+    if 'x' in out.keys():
+        del out['x']
+    if 'xdot' in out.keys():
+        del out['xdot']
+    if 'u' in out.keys():
+        del out['u']
+    if 'z' in out.keys():
+        del out['z']
+    if 'p' in out.keys():
+        del out['p']
+    # constraints
+    if 'con_phi_expr' in out.keys():
+        del out['con_phi_expr']
+    if 'con_h_expr' in out.keys():
+        del out['con_h_expr']
+    if 'con_r_expr' in out.keys():
+        del out['con_r_expr']
+    if 'con_r_in_phi' in out.keys():
+        del out['con_r_in_phi']
+    # terminal
+    if 'con_phi_expr_e' in out.keys():
+        del out['con_phi_expr_e']
+    if 'con_h_expr_e' in out.keys():
+        del out['con_h_expr_e']
+    if 'con_r_expr_e' in out.keys():
+        del out['con_r_expr_e']
+    if 'con_r_in_phi_e' in out.keys():
+        del out['con_r_in_phi_e']
+    # cost
+    if 'cost_y_expr' in out.keys():
+        del out['cost_y_expr']
+    if 'cost_y_expr_e' in out.keys():
+        del out['cost_y_expr_e']
+    if 'cost_y_expr_0' in out.keys():
+        del out['cost_y_expr_0']
+    if 'cost_expr_ext_cost' in out.keys():
+        del out['cost_expr_ext_cost']
+    if 'cost_expr_ext_cost_e' in out.keys():
+        del out['cost_expr_ext_cost_e']
+    if 'cost_expr_ext_cost_0' in out.keys():
+        del out['cost_expr_ext_cost_0']
+
+    return out
diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py
new file mode 100644
index 000000000..9c2f02aa8
--- /dev/null
+++ b/pyextra/acados_template/acados_ocp.py
@@ -0,0 +1,2823 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import numpy as np
+import os
+from .acados_model import AcadosModel
+from .utils import get_acados_path, J_to_idx, J_to_idx_slack
+
+class AcadosOcpDims:
+    """
+    Class containing the dimensions of the optimal control problem.
+    """
+    def __init__(self):
+        self.__nx      = None
+        self.__nu      = None
+        self.__nz      = 0
+        self.__np      = 0
+        self.__ny      = 0
+        self.__ny_e    = 0
+        self.__ny_0    = 0
+        self.__nr      = 0
+        self.__nr_e    = 0
+        self.__nh      = 0
+        self.__nh_e    = 0
+        self.__nphi    = 0
+        self.__nphi_e  = 0
+        self.__nbx     = 0
+        self.__nbx_0   = 0
+        self.__nbx_e   = 0
+        self.__nbu     = 0
+        self.__nsbx    = 0
+        self.__nsbx_e  = 0
+        self.__nsbu    = 0
+        self.__nsh     = 0
+        self.__nsh_e   = 0
+        self.__nsphi   = 0
+        self.__nsphi_e = 0
+        self.__ns      = 0
+        self.__ns_e    = 0
+        self.__ng      = 0
+        self.__ng_e    = 0
+        self.__nsg     = 0
+        self.__nsg_e   = 0
+        self.__nbxe_0  = None
+        self.__N       = None
+
+
+    @property
+    def nx(self):
+        """:math:`n_x` - number of states.
+        Type: int; default: None"""
+        return self.__nx
+
+    @property
+    def nz(self):
+        """:math:`n_z` - number of algebraic variables.
+        Type: int; default: 0"""
+        return self.__nz
+
+    @property
+    def nu(self):
+        """:math:`n_u` - number of inputs.
+        Type: int; default: None"""
+        return self.__nu
+
+    @property
+    def np(self):
+        """:math:`n_p` - number of parameters.
+        Type: int; default: 0"""
+        return self.__np
+
+    @property
+    def ny(self):
+        """:math:`n_y` - number of residuals in Lagrange term.
+        Type: int; default: 0"""
+        return self.__ny
+
+    @property
+    def ny_0(self):
+        """:math:`n_{y}^0` - number of residuals in Mayer term.
+        Type: int; default: 0"""
+        return self.__ny_0
+
+    @property
+    def ny_e(self):
+        """:math:`n_{y}^e` - number of residuals in Mayer term.
+        Type: int; default: 0"""
+        return self.__ny_e
+
+    @property
+    def nr(self):
+        """:math:`n_{\pi}` - dimension of the image of the inner nonlinear function in positive definite constraints.
+        Type: int; default: 0"""
+        return self.__nr
+
+    @property
+    def nr_e(self):
+        """:math:`n_{\pi}^e` - dimension of the image of the inner nonlinear function in positive definite constraints.
+        Type: int; default: 0"""
+        return self.__nr_e
+
+    @property
+    def nh(self):
+        """:math:`n_h` - number of nonlinear constraints.
+        Type: int; default: 0"""
+        return self.__nh
+
+    @property
+    def nh_e(self):
+        """:math:`n_{h}^e` - number of nonlinear constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nh_e
+
+    @property
+    def nphi(self):
+        """:math:`n_{\phi}` - number of convex-over-nonlinear constraints.
+        Type: int; default: 0"""
+        return self.__nphi
+
+    @property
+    def nphi_e(self):
+        """:math:`n_{\phi}^e` - number of convex-over-nonlinear constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nphi_e
+
+    @property
+    def nbx(self):
+        """:math:`n_{b_x}` - number of state bounds.
+        Type: int; default: 0"""
+        return self.__nbx
+
+    @property
+    def nbxe_0(self):
+        """:math:`n_{be_{x0}}` - number of state bounds at initial shooting node that are equalities.
+        Type: int; default: None"""
+        return self.__nbxe_0
+
+    @property
+    def nbx_0(self):
+        """:math:`n_{b_{x0}}` - number of state bounds for initial state.
+        Type: int; default: 0"""
+        return self.__nbx_0
+
+    @property
+    def nbx_e(self):
+        """:math:`n_{b_x}` - number of state bounds at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nbx_e
+
+    @property
+    def nbu(self):
+        """:math:`n_{b_u}` - number of input bounds.
+        Type: int; default: 0"""
+        return self.__nbu
+
+    @property
+    def nsbx(self):
+        """:math:`n_{{sb}_x}` - number of soft state bounds.
+        Type: int; default: 0"""
+        return self.__nsbx
+
+    @property
+    def nsbx_e(self):
+        """:math:`n_{{sb}^e_{x}}` - number of soft state bounds at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nsbx_e
+
+    @property
+    def nsbu(self):
+        """:math:`n_{{sb}_u}` - number of soft input bounds.
+        Type: int; default: 0"""
+        return self.__nsbu
+
+    @property
+    def nsg(self):
+        """:math:`n_{{sg}}` - number of soft general linear constraints.
+        Type: int; default: 0"""
+        return self.__nsg
+
+    @property
+    def nsg_e(self):
+        """:math:`n_{{sg}^e}` - number of soft general linear constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nsg_e
+
+    @property
+    def nsh(self):
+        """:math:`n_{{sh}}` - number of soft nonlinear constraints.
+        Type: int; default: 0"""
+        return self.__nsh
+
+    @property
+    def nsh_e(self):
+        """:math:`n_{{sh}}^e` - number of soft nonlinear constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nsh_e
+
+    @property
+    def nsphi(self):
+        """:math:`n_{{s\phi}}` - number of soft convex-over-nonlinear constraints.
+        Type: int; default: 0"""
+        return self.__nsphi
+
+    @property
+    def nsphi_e(self):
+        """:math:`n_{{s\phi}^e}` - number of soft convex-over-nonlinear constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__nsphi_e
+
+    @property
+    def ns(self):
+        """:math:`n_{s}` - total number of slacks.
+        Type: int; default: 0"""
+        return self.__ns
+
+    @property
+    def ns_e(self):
+        """:math:`n_{s}^e` - total number of slacks at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__ns_e
+
+    @property
+    def ng(self):
+        """:math:`n_{g}` - number of general polytopic constraints.
+        Type: int; default: 0"""
+        return self.__ng
+
+    @property
+    def ng_e(self):
+        """:math:`n_{g}^e` - number of general polytopic constraints at terminal shooting node N.
+        Type: int; default: 0"""
+        return self.__ng_e
+
+    @property
+    def N(self):
+        """:math:`N` - prediction horizon.
+        Type: int; default: None"""
+        return self.__N
+
+    @nx.setter
+    def nx(self, nx):
+        if type(nx) == int and nx > 0:
+            self.__nx = nx
+        else:
+            raise Exception('Invalid nx value, expected positive integer. Exiting.')
+
+    @nz.setter
+    def nz(self, nz):
+        if type(nz) == int and nz > -1:
+            self.__nz = nz
+        else:
+            raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
+
+    @nu.setter
+    def nu(self, nu):
+        if type(nu) == int and nu > -1:
+            self.__nu = nu
+        else:
+            raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
+
+    @np.setter
+    def np(self, np):
+        if type(np) == int and np > -1:
+            self.__np = np
+        else:
+            raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
+
+    @ny_0.setter
+    def ny_0(self, ny_0):
+        if isinstance(ny_0, int) and ny_0 > -1:
+            self.__ny_0 = ny_0
+        else:
+            raise Exception('Invalid ny_0 value, expected nonnegative integer. Exiting.')
+
+    @ny.setter
+    def ny(self, ny):
+        if isinstance(ny, int) and ny > -1:
+            self.__ny = ny
+        else:
+            raise Exception('Invalid ny value, expected nonnegative integer. Exiting.')
+
+    @ny_e.setter
+    def ny_e(self, ny_e):
+        if type(ny_e) == int and ny_e > -1:
+            self.__ny_e = ny_e
+        else:
+            raise Exception('Invalid ny_e value, expected nonnegative integer. Exiting.')
+
+    @nr.setter
+    def nr(self, nr):
+        if type(nr) == int and nr > -1:
+            self.__nr = nr
+        else:
+            raise Exception('Invalid nr value, expected nonnegative integer. Exiting.')
+
+    @nr_e.setter
+    def nr_e(self, nr_e):
+        if type(nr_e) == int and nr_e > -1:
+            self.__nr_e = nr_e
+        else:
+            raise Exception('Invalid nr_e value, expected nonnegative integer. Exiting.')
+
+    @nh.setter
+    def nh(self, nh):
+        if type(nh) == int and nh > -1:
+            self.__nh = nh
+        else:
+            raise Exception('Invalid nh value, expected nonnegative integer. Exiting.')
+
+    @nh_e.setter
+    def nh_e(self, nh_e):
+        if type(nh_e) == int and nh_e > -1:
+            self.__nh_e = nh_e
+        else:
+            raise Exception('Invalid nh_e value, expected nonnegative integer. Exiting.')
+
+    @nphi.setter
+    def nphi(self, nphi):
+        if type(nphi) == int and nphi > -1:
+            self.__nphi = nphi
+        else:
+            raise Exception('Invalid nphi value, expected nonnegative integer. Exiting.')
+
+    @nphi_e.setter
+    def nphi_e(self, nphi_e):
+        if type(nphi_e) == int and nphi_e > -1:
+            self.__nphi_e = nphi_e
+        else:
+            raise Exception('Invalid nphi_e value, expected nonnegative integer. Exiting.')
+
+    @nbx.setter
+    def nbx(self, nbx):
+        if isinstance(nbx, int) and nbx > -1:
+            self.__nbx = nbx
+        else:
+            raise Exception('Invalid nbx value, expected nonnegative integer. Exiting.')
+
+    @nbxe_0.setter
+    def nbxe_0(self, nbxe_0):
+        if isinstance(nbxe_0, int) and nbxe_0 > -1:
+            self.__nbxe_0 = nbxe_0
+        else:
+            raise Exception('Invalid nbxe_0 value, expected nonnegative integer. Exiting.')
+
+    @nbx_0.setter
+    def nbx_0(self, nbx_0):
+        if type(nbx_0) == int and nbx_0 > -1:
+            self.__nbx_0 = nbx_0
+        else:
+            raise Exception('Invalid nbx_0 value, expected nonnegative integer. Exiting.')
+
+    @nbx_e.setter
+    def nbx_e(self, nbx_e):
+        if type(nbx_e) == int and nbx_e > -1:
+            self.__nbx_e = nbx_e
+        else:
+            raise Exception('Invalid nbx_e value, expected nonnegative integer. Exiting.')
+
+    @nbu.setter
+    def nbu(self, nbu):
+        if type(nbu) == int and nbu > -1:
+            self.__nbu = nbu
+        else:
+            raise Exception('Invalid nbu value, expected nonnegative integer. Exiting.')
+
+    @nsbx.setter
+    def nsbx(self, nsbx):
+        if type(nsbx) == int and nsbx > -1:
+            self.__nsbx = nsbx
+        else:
+            raise Exception('Invalid nsbx value, expected nonnegative integer. Exiting.')
+
+    @nsbx_e.setter
+    def nsbx_e(self, nsbx_e):
+        if type(nsbx_e) == int and nsbx_e > -1:
+            self.__nsbx_e = nsbx_e
+        else:
+            raise Exception('Invalid nsbx_e value, expected nonnegative integer. Exiting.')
+
+    @nsbu.setter
+    def nsbu(self, nsbu):
+        if type(nsbu) == int and nsbu > -1:
+            self.__nsbu = nsbu
+        else:
+            raise Exception('Invalid nsbu value, expected nonnegative integer. Exiting.')
+
+    @nsg.setter
+    def nsg(self, nsg):
+        if isinstance(nsg, int) and nsg > -1:
+            self.__nsg = nsg
+        else:
+            raise Exception('Invalid nsg value, expected nonnegative integer. Exiting.')
+
+    @nsg_e.setter
+    def nsg_e(self, nsg_e):
+        if isinstance(nsg_e, int) and nsg_e > -1:
+            self.__nsg_e = nsg_e
+        else:
+            raise Exception('Invalid nsg_e value, expected nonnegative integer. Exiting.')
+
+    @nsh.setter
+    def nsh(self, nsh):
+        if isinstance(nsh, int) and nsh > -1:
+            self.__nsh = nsh
+        else:
+            raise Exception('Invalid nsh value, expected nonnegative integer. Exiting.')
+
+    @nsh_e.setter
+    def nsh_e(self, nsh_e):
+        if isinstance(nsh_e, int) and nsh_e > -1:
+            self.__nsh_e = nsh_e
+        else:
+            raise Exception('Invalid nsh_e value, expected nonnegative integer. Exiting.')
+
+    @nsphi.setter
+    def nsphi(self, nsphi):
+        if isinstance(nsphi, int) and nsphi > -1:
+            self.__nsphi = nsphi
+        else:
+            raise Exception('Invalid nsphi value, expected nonnegative integer. Exiting.')
+
+    @nsphi_e.setter
+    def nsphi_e(self, nsphi_e):
+        if isinstance(nsphi_e, int) and nsphi_e > -1:
+            self.__nsphi_e = nsphi_e
+        else:
+            raise Exception('Invalid nsphi_e value, expected nonnegative integer. Exiting.')
+
+    @ns.setter
+    def ns(self, ns):
+        if isinstance(ns, int) and ns > -1:
+            self.__ns = ns
+        else:
+            raise Exception('Invalid ns value, expected nonnegative integer. Exiting.')
+
+    @ns_e.setter
+    def ns_e(self, ns_e):
+        if isinstance(ns_e, int) and ns_e > -1:
+            self.__ns_e = ns_e
+        else:
+            raise Exception('Invalid ns_e value, expected nonnegative integer. Exiting.')
+
+    @ng.setter
+    def ng(self, ng):
+        if isinstance(ng, int) and ng > -1:
+            self.__ng = ng
+        else:
+            raise Exception('Invalid ng value, expected nonnegative integer. Exiting.')
+
+    @ng_e.setter
+    def ng_e(self, ng_e):
+        if isinstance(ng_e, int) and ng_e > -1:
+            self.__ng_e = ng_e
+        else:
+            raise Exception('Invalid ng_e value, expected nonnegative integer. Exiting.')
+
+    @N.setter
+    def N(self, N):
+        if isinstance(N, int) and N > 0:
+            self.__N = N
+        else:
+            raise Exception('Invalid N value, expected positive integer. Exiting.')
+
+    def set(self, attr, value):
+        setattr(self, attr, value)
+
+
+class AcadosOcpCost:
+    """
+    Class containing the numerical data of the cost:
+
+    In case of LINEAR_LS:
+    stage cost is
+    :math:`l(x,u,z) = || V_x \, x + V_u \, u + V_z \, z - y_\\text{ref}||^2_W`,
+    terminal cost is
+    :math:`m(x) = || V^e_x \, x - y_\\text{ref}^e||^2_{W^e}`
+
+    In case of NONLINEAR_LS:
+    stage cost is
+    :math:`l(x,u,z) = || y(x,u,z) - y_\\text{ref}||^2_W`,
+    terminal cost is
+    :math:`m(x) = || y^e(x) - y_\\text{ref}^e||^2_{W^e}`
+    """
+    def __init__(self):
+        # initial stage
+        self.__cost_type_0 = None
+        self.__W_0 = None
+        self.__Vx_0 = None
+        self.__Vu_0 = None
+        self.__Vz_0 = None
+        self.__yref_0 = None
+        self.__cost_ext_fun_type_0 = 'casadi'
+        # Lagrange term
+        self.__cost_type   = 'LINEAR_LS'  # cost type
+        self.__W           = np.zeros((0,0))
+        self.__Vx          = np.zeros((0,0))
+        self.__Vu          = np.zeros((0,0))
+        self.__Vz          = np.zeros((0,0))
+        self.__yref        = np.array([])
+        self.__Zl          = np.array([])
+        self.__Zu          = np.array([])
+        self.__zl          = np.array([])
+        self.__zu          = np.array([])
+        self.__cost_ext_fun_type = 'casadi'
+        # Mayer term
+        self.__cost_type_e = 'LINEAR_LS'
+        self.__W_e         = np.zeros((0,0))
+        self.__Vx_e        = np.zeros((0,0))
+        self.__yref_e      = np.array([])
+        self.__Zl_e        = np.array([])
+        self.__Zu_e        = np.array([])
+        self.__zl_e        = np.array([])
+        self.__zu_e        = np.array([])
+        self.__cost_ext_fun_type_e = 'casadi'
+
+    # initial stage
+    @property
+    def cost_type_0(self):
+        """Cost type at initial shooting node (0)
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS} or :code:`None`.
+        Default: :code:`None`.
+
+            .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly.
+
+            .. note:: If :py:attr:`cost_type_0` is set to :code:`None` values in :py:attr:`W_0`, :py:attr:`Vx_0`, :py:attr:`Vu_0`, :py:attr:`Vz_0` and :py:attr:`yref_0` are ignored (set to :code:`None`).
+        """
+        return self.__cost_type_0
+
+    @property
+    def W_0(self):
+        """:math:`W_0` - weight matrix at initial shooting node (0).
+        Default: :code:`None`.
+        """
+        return self.__W_0
+
+    @property
+    def Vx_0(self):
+        """:math:`V_x^0` - x matrix coefficient at initial shooting node (0).
+        Default: :code:`None`.
+        """
+        return self.__Vx_0
+
+    @property
+    def Vu_0(self):
+        """:math:`V_u^0` - u matrix coefficient at initial shooting node (0).
+        Default: :code:`None`.
+        """
+        return self.__Vu_0
+
+    @property
+    def Vz_0(self):
+        """:math:`V_z^0` - z matrix coefficient at initial shooting node (0).
+        Default: :code:`None`.
+        """
+        return self.__Vz_0
+
+    @property
+    def yref_0(self):
+        """:math:`y_\\text{ref}^0` - reference at initial shooting node (0).
+        Default: :code:`None`.
+        """
+        return self.__yref_0
+
+    @property
+    def cost_ext_fun_type_0(self):
+        """Type of external function for cost at initial shooting node (0)
+        -- string in {casadi, generic} or :code:`None`
+        Default: :code:'casadi'.
+
+            .. note:: Cost at initial stage is the same as for intermediate shooting nodes if not set differently explicitly.
+        """
+        return self.__cost_ext_fun_type_0
+
+    @yref_0.setter
+    def yref_0(self, yref_0):
+        if isinstance(yref_0, np.ndarray):
+            self.__yref_0 = yref_0
+        else:
+            raise Exception('Invalid yref_0 value, expected numpy array. Exiting.')
+
+    @W_0.setter
+    def W_0(self, W_0):
+        if isinstance(W_0, np.ndarray) and len(W_0.shape) == 2:
+            self.__W_0 = W_0
+        else:
+            raise Exception('Invalid cost W_0 value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vx_0.setter
+    def Vx_0(self, Vx_0):
+        if isinstance(Vx_0, np.ndarray) and len(Vx_0.shape) == 2:
+            self.__Vx_0 = Vx_0
+        else:
+            raise Exception('Invalid cost Vx_0 value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vu_0.setter
+    def Vu_0(self, Vu_0):
+        if isinstance(Vu_0, np.ndarray) and len(Vu_0.shape) == 2:
+            self.__Vu_0 = Vu_0
+        else:
+            raise Exception('Invalid cost Vu_0 value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vz_0.setter
+    def Vz_0(self, Vz_0):
+        if isinstance(Vz_0, np.ndarray) and len(Vz_0.shape) == 2:
+            self.__Vz_0 = Vz_0
+        else:
+            raise Exception('Invalid cost Vz_0 value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @cost_ext_fun_type_0.setter
+    def cost_ext_fun_type_0(self, cost_ext_fun_type_0):
+        if cost_ext_fun_type_0 in ['casadi', 'generic']:
+            self.__cost_ext_fun_type_0 = cost_ext_fun_type_0
+        else:
+            raise Exception('Invalid cost_ext_fun_type_0 value, expected numpy array. Exiting.')
+
+    # Lagrange term
+    @property
+    def cost_type(self):
+        """
+        Cost type at intermediate shooting nodes (1 to N-1)
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}.
+        Default: 'LINEAR_LS'.
+        """
+        return self.__cost_type
+
+    @property
+    def W(self):
+        """:math:`W` - weight matrix at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__W
+
+    @property
+    def Vx(self):
+        """:math:`V_x` - x matrix coefficient at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__Vx
+
+    @property
+    def Vu(self):
+        """:math:`V_u` - u matrix coefficient at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__Vu
+
+    @property
+    def Vz(self):
+        """:math:`V_z` - z matrix coefficient at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__Vz
+
+    @property
+    def yref(self):
+        """:math:`y_\\text{ref}` - reference at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.array([])`.
+        """
+        return self.__yref
+
+    @property
+    def Zl(self):
+        """:math:`Z_l` - diagonal of Hessian wrt lower slack at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.array([])`.
+        """
+        return self.__Zl
+
+    @property
+    def Zu(self):
+        """:math:`Z_u` - diagonal of Hessian wrt upper slack at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.array([])`.
+        """
+        return self.__Zu
+
+    @property
+    def zl(self):
+        """:math:`z_l` - gradient wrt lower slack at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.array([])`.
+        """
+        return self.__zl
+
+    @property
+    def zu(self):
+        """:math:`z_u` - gradient wrt upper slack at intermediate shooting nodes (1 to N-1).
+        Default: :code:`np.array([])`.
+        """
+        return self.__zu
+
+    @property
+    def cost_ext_fun_type(self):
+        """Type of external function for cost at intermediate shooting nodes (1 to N-1).
+        -- string in {casadi, generic}
+        Default: :code:'casadi'.
+        """
+        return self.__cost_ext_fun_type
+
+    @cost_type.setter
+    def cost_type(self, cost_type):
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+        if cost_type in cost_types:
+            self.__cost_type = cost_type
+        else:
+            raise Exception('Invalid cost_type value. Exiting.')
+
+    @cost_type_0.setter
+    def cost_type_0(self, cost_type_0):
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+        if cost_type_0 in cost_types:
+            self.__cost_type_0 = cost_type_0
+        else:
+            raise Exception('Invalid cost_type_0 value. Exiting.')
+
+    @W.setter
+    def W(self, W):
+        if isinstance(W, np.ndarray) and len(W.shape) == 2:
+            self.__W = W
+        else:
+            raise Exception('Invalid cost W value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+
+    @Vx.setter
+    def Vx(self, Vx):
+        if isinstance(Vx, np.ndarray) and len(Vx.shape) == 2:
+            self.__Vx = Vx
+        else:
+            raise Exception('Invalid cost Vx value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vu.setter
+    def Vu(self, Vu):
+        if isinstance(Vu, np.ndarray) and len(Vu.shape) == 2:
+            self.__Vu = Vu
+        else:
+            raise Exception('Invalid cost Vu value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vz.setter
+    def Vz(self, Vz):
+        if isinstance(Vz, np.ndarray) and len(Vz.shape) == 2:
+            self.__Vz = Vz
+        else:
+            raise Exception('Invalid cost Vz value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @yref.setter
+    def yref(self, yref):
+        if isinstance(yref, np.ndarray):
+            self.__yref = yref
+        else:
+            raise Exception('Invalid yref value, expected numpy array. Exiting.')
+
+    @Zl.setter
+    def Zl(self, Zl):
+        if isinstance(Zl, np.ndarray):
+            self.__Zl = Zl
+        else:
+            raise Exception('Invalid Zl value, expected numpy array. Exiting.')
+
+    @Zu.setter
+    def Zu(self, Zu):
+        if isinstance(Zu, np.ndarray):
+            self.__Zu = Zu
+        else:
+            raise Exception('Invalid Zu value, expected numpy array. Exiting.')
+
+    @zl.setter
+    def zl(self, zl):
+        if isinstance(zl, np.ndarray):
+            self.__zl = zl
+        else:
+            raise Exception('Invalid zl value, expected numpy array. Exiting.')
+
+    @zu.setter
+    def zu(self, zu):
+        if isinstance(zu, np.ndarray):
+            self.__zu = zu
+        else:
+            raise Exception('Invalid zu value, expected numpy array. Exiting.')
+
+    @cost_ext_fun_type.setter
+    def cost_ext_fun_type(self, cost_ext_fun_type):
+        if cost_ext_fun_type in ['casadi', 'generic']:
+            self.__cost_ext_fun_type = cost_ext_fun_type
+        else:
+            raise Exception('Invalid cost_ext_fun_type value, expected numpy array. Exiting.')
+
+    # Mayer term
+    @property
+    def cost_type_e(self):
+        """
+        Cost type at terminal shooting node (N)
+        -- string in {EXTERNAL, LINEAR_LS, NONLINEAR_LS}.
+        Default: 'LINEAR_LS'.
+        """
+        return self.__cost_type_e
+
+    @property
+    def W_e(self):
+        """:math:`W_e` - weight matrix at terminal shooting node (N).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__W_e
+
+    @property
+    def Vx_e(self):
+        """:math:`V_x^e` - x matrix coefficient for cost at terminal shooting node (N).
+        Default: :code:`np.zeros((0,0))`.
+        """
+        return self.__Vx_e
+
+    @property
+    def yref_e(self):
+        """:math:`y_\\text{ref}^e` - cost reference at terminal shooting node (N).
+        Default: :code:`np.array([])`.
+        """
+        return self.__yref_e
+
+    @property
+    def Zl_e(self):
+        """:math:`Z_l^e` - diagonal of Hessian wrt lower slack at terminal shooting node (N).
+        Default: :code:`np.array([])`.
+        """
+        return self.__Zl_e
+
+    @property
+    def Zu_e(self):
+        """:math:`Z_u^e` - diagonal of Hessian wrt upper slack at terminal shooting node (N).
+        Default: :code:`np.array([])`.
+        """
+        return self.__Zu_e
+
+    @property
+    def zl_e(self):
+        """:math:`z_l^e` - gradient wrt lower slack at terminal shooting node (N).
+        Default: :code:`np.array([])`.
+        """
+        return self.__zl_e
+
+    @property
+    def zu_e(self):
+        """:math:`z_u^e` - gradient wrt upper slack at terminal shooting node (N).
+        Default: :code:`np.array([])`.
+        """
+        return self.__zu_e
+
+    @property
+    def cost_ext_fun_type_e(self):
+        """Type of external function for cost at intermediate shooting nodes (1 to N-1).
+        -- string in {casadi, generic}
+        Default: :code:'casadi'.
+        """
+        return self.__cost_ext_fun_type_e
+
+    @cost_type_e.setter
+    def cost_type_e(self, cost_type_e):
+        cost_types = ('LINEAR_LS', 'NONLINEAR_LS', 'EXTERNAL')
+
+        if cost_type_e in cost_types:
+            self.__cost_type_e = cost_type_e
+        else:
+            raise Exception('Invalid cost_type_e value. Exiting.')
+
+    @W_e.setter
+    def W_e(self, W_e):
+        if isinstance(W_e, np.ndarray) and len(W_e.shape) == 2:
+            self.__W_e = W_e
+        else:
+            raise Exception('Invalid cost W_e value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @Vx_e.setter
+    def Vx_e(self, Vx_e):
+        if isinstance(Vx_e, np.ndarray) and len(Vx_e.shape) == 2:
+            self.__Vx_e = Vx_e
+        else:
+            raise Exception('Invalid cost Vx_e value. ' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @yref_e.setter
+    def yref_e(self, yref_e):
+        if isinstance(yref_e, np.ndarray):
+            self.__yref_e = yref_e
+        else:
+            raise Exception('Invalid yref_e value, expected numpy array. Exiting.')
+
+    @Zl_e.setter
+    def Zl_e(self, Zl_e):
+        if isinstance(Zl_e, np.ndarray):
+            self.__Zl_e = Zl_e
+        else:
+            raise Exception('Invalid Zl_e value, expected numpy array. Exiting.')
+
+    @Zu_e.setter
+    def Zu_e(self, Zu_e):
+        if isinstance(Zu_e, np.ndarray):
+            self.__Zu_e = Zu_e
+        else:
+            raise Exception('Invalid Zu_e value, expected numpy array. Exiting.')
+
+    @zl_e.setter
+    def zl_e(self, zl_e):
+        if isinstance(zl_e, np.ndarray):
+            self.__zl_e = zl_e
+        else:
+            raise Exception('Invalid zl_e value, expected numpy array. Exiting.')
+
+    @zu_e.setter
+    def zu_e(self, zu_e):
+        if isinstance(zu_e, np.ndarray):
+            self.__zu_e = zu_e
+        else:
+            raise Exception('Invalid zu_e value, expected numpy array. Exiting.')
+
+    @cost_ext_fun_type_e.setter
+    def cost_ext_fun_type_e(self, cost_ext_fun_type_e):
+        if cost_ext_fun_type_e in ['casadi', 'generic']:
+            self.__cost_ext_fun_type_e = cost_ext_fun_type_e
+        else:
+            raise Exception('Invalid cost_ext_fun_type_e value, expected numpy array. Exiting.')
+
+    def set(self, attr, value):
+        setattr(self, attr, value)
+
+
+def print_J_to_idx_note():
+    print("NOTE: J* matrix is converted to zero based vector idx* vector, which is returned here.")
+
+
+class AcadosOcpConstraints:
+    """
+    class containing the description of the constraints
+    """
+    def __init__(self):
+        self.__constr_type   = 'BGH'
+        self.__constr_type_e = 'BGH'
+        # initial x
+        self.__lbx_0   = np.array([])
+        self.__ubx_0   = np.array([])
+        self.__idxbx_0 = np.array([])
+        self.__idxbxe_0 = np.array([])
+        # state bounds
+        self.__lbx     = np.array([])
+        self.__ubx     = np.array([])
+        self.__idxbx   = np.array([])
+        # bounds on x at shooting node N
+        self.__lbx_e   = np.array([])
+        self.__ubx_e   = np.array([])
+        self.__idxbx_e = np.array([])
+        # bounds on u
+        self.__lbu     = np.array([])
+        self.__ubu     = np.array([])
+        self.__idxbu   = np.array([])
+        # polytopic constraints
+        self.__lg      = np.array([])
+        self.__ug      = np.array([])
+        self.__D       = np.zeros((0,0))
+        self.__C       = np.zeros((0,0))
+        # polytopic constraints at shooting node N
+        self.__C_e     = np.zeros((0,0))
+        self.__lg_e    = np.array([])
+        self.__ug_e    = np.array([])
+        # nonlinear constraints
+        self.__lh      = np.array([])
+        self.__uh      = np.array([])
+        # nonlinear constraints at shooting node N
+        self.__uh_e    = np.array([])
+        self.__lh_e    = np.array([])
+        # convex-over-nonlinear constraints
+        self.__lphi    = np.array([])
+        self.__uphi    = np.array([])
+        # nonlinear constraints at shooting node N
+        self.__uphi_e = np.array([])
+        self.__lphi_e = np.array([])
+        # SLACK BOUNDS
+        # soft bounds on x
+        self.__lsbx   = np.array([])
+        self.__usbx   = np.array([])
+        self.__idxsbx = np.array([])
+        # soft bounds on u
+        self.__lsbu   = np.array([])
+        self.__usbu   = np.array([])
+        self.__idxsbu = np.array([])
+        # soft bounds on x at shooting node N
+        self.__lsbx_e  = np.array([])
+        self.__usbx_e  = np.array([])
+        self.__idxsbx_e= np.array([])
+        # soft bounds on general linear constraints
+        self.__lsg    = np.array([])
+        self.__usg    = np.array([])
+        self.__idxsg  = np.array([])
+        # soft bounds on nonlinear constraints
+        self.__lsh    = np.array([])
+        self.__ush    = np.array([])
+        self.__idxsh  = np.array([])
+        # soft bounds on nonlinear constraints
+        self.__lsphi  = np.array([])
+        self.__usphi  = np.array([])
+        self.__idxsphi  = np.array([])
+        # soft bounds on general linear constraints at shooting node N
+        self.__lsg_e    = np.array([])
+        self.__usg_e    = np.array([])
+        self.__idxsg_e  = np.array([])
+        # soft bounds on nonlinear constraints at shooting node N
+        self.__lsh_e    = np.array([])
+        self.__ush_e    = np.array([])
+        self.__idxsh_e  = np.array([])
+        # soft bounds on nonlinear constraints at shooting node N
+        self.__lsphi_e    = np.array([])
+        self.__usphi_e    = np.array([])
+        self.__idxsphi_e  = np.array([])
+
+
+    # types
+    @property
+    def constr_type(self):
+        """Constraints type for shooting nodes (0 to N-1). string in {BGH, BGP}.
+        Default: BGH; BGP is for convex over nonlinear."""
+        return self.__constr_type
+
+    @property
+    def constr_type_e(self):
+        """Constraints type for terminal shooting node N. string in {BGH, BGP}.
+        Default: BGH; BGP is for convex over nonlinear."""
+        return self.__constr_type_e
+
+    # initial bounds on x
+    @property
+    def lbx_0(self):
+        """:math:`\\underline{x_0}` - lower bounds on x at initial stage 0.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`."""
+        return self.__lbx_0
+
+    @property
+    def ubx_0(self):
+        """:math:`\\bar{x_0}` - upper bounds on x at initial stage 0.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__ubx_0
+
+    @property
+    def Jbx_0(self):
+        """:math:`J_{bx,0}` - matrix coefficient for bounds on x at initial stage 0.
+        Translated internally to :py:attr:`idxbx_0`"""
+        print_J_to_idx_note()
+        return self.__idxbx_0
+
+    @property
+    def idxbx_0(self):
+        """Indices of bounds on x at initial stage 0
+        -- can be set automatically via x0.
+        Can be set by using :py:attr:`Jbx_0`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxbx_0
+
+    @property
+    def idxbxe_0(self):
+        """Indices of bounds on x0 that are equalities -- can be set automatically via :py:attr:`x0`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxbxe_0
+
+    # bounds on x
+    @property
+    def lbx(self):
+        """:math:`\\underline{x}` - lower bounds on x at intermediate shooting nodes (1 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__lbx
+
+    @property
+    def ubx(self):
+        """:math:`\\bar{x}` - upper bounds on x at intermediate shooting nodes (1 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__ubx
+
+    @property
+    def idxbx(self):
+        """indices of bounds on x (defines :math:`J_{bx}`) at intermediate shooting nodes (1 to N-1).
+        Can be set by using :py:attr:`Jbx`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxbx
+
+    @property
+    def Jbx(self):
+        """:math:`J_{bx}` - matrix coefficient for bounds on x
+        at intermediate shooting nodes (1 to N-1).
+        Translated internally into :py:attr:`idxbx`."""
+        print_J_to_idx_note()
+        return self.__idxbx
+
+    # bounds on x at shooting node N
+    @property
+    def lbx_e(self):
+        """:math:`\\underline{x}^e` - lower bounds on x at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__lbx_e
+
+    @property
+    def ubx_e(self):
+        """:math:`\\bar{x}^e` - upper bounds on x at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__ubx_e
+
+    @property
+    def idxbx_e(self):
+        """Indices for bounds on x at terminal shooting node N (defines :math:`J_{bx}^e`).
+        Can be set by using :py:attr:`Jbx_e`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxbx_e
+
+    @property
+    def Jbx_e(self):
+        """:math:`J_{bx}^e` matrix coefficient for bounds on x at terminal shooting node N.
+        Translated internally into :py:attr:`idxbx_e`."""
+        print_J_to_idx_note()
+        return self.__idxbx_e
+
+    # bounds on u
+    @property
+    def lbu(self):
+        """:math:`\\underline{u}` - lower bounds on u at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`
+        """
+        return self.__lbu
+
+    @property
+    def ubu(self):
+        """:math:`\\bar{u}` - upper bounds on u at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`
+        """
+        return self.__ubu
+
+    @property
+    def idxbu(self):
+        """Indices of bounds on u (defines :math:`J_{bu}`) at shooting nodes (0 to N-1).
+        Can be set by using :py:attr:`Jbu`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`
+        """
+        return self.__idxbu
+
+    @property
+    def Jbu(self):
+        """:math:`J_{bu}` - matrix coefficient for bounds on u at shooting nodes (0 to N-1).
+        Translated internally to :py:attr:`idxbu`.
+        """
+        print_J_to_idx_note()
+        return self.__idxbu
+
+    # polytopic constraints
+    @property
+    def C(self):
+        """:math:`C` - C matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`.
+        """
+        return self.__C
+
+    @property
+    def D(self):
+        """:math:`D` - D matrix in :math:`\\underline{g} \\leq D \, u + C \, x \\leq \\bar{g}`
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`
+        """
+        return self.__D
+
+    @property
+    def lg(self):
+        """:math:`\\underline{g}` - lower bound for general polytopic inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`
+        """
+        return self.__lg
+
+    @property
+    def ug(self):
+        """:math:`\\bar{g}` - upper bound for general polytopic inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__ug
+
+    # polytopic constraints at shooting node N
+    @property
+    def C_e(self):
+        """:math:`C^e` - C matrix at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array((0,0))`.
+        """
+        return self.__C_e
+
+    @property
+    def lg_e(self):
+        """:math:`\\underline{g}^e` - lower bound on general polytopic inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__lg_e
+
+    @property
+    def ug_e(self):
+        """:math:`\\bar{g}^e` - upper bound on general polytopic inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__ug_e
+
+
+    # nonlinear constraints
+    @property
+    def lh(self):
+        """:math:`\\underline{h}` - lower bound for nonlinear inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__lh
+
+    @property
+    def uh(self):
+        """:math:`\\bar{h}` - upper bound for nonlinear inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__uh
+
+    # nonlinear constraints at shooting node N
+    @property
+    def lh_e(self):
+        """:math:`\\underline{h}^e` - lower bound on nonlinear inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__lh_e
+
+    @property
+    def uh_e(self):
+        """:math:`\\bar{h}^e` - upper bound on nonlinear inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__uh_e
+
+    # convex-over-nonlinear constraints
+    @property
+    def lphi(self):
+        """:math:`\\underline{\phi}` - lower bound for convex-over-nonlinear inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__lphi
+
+    @property
+    def uphi(self):
+        """:math:`\\bar{\phi}` - upper bound for convex-over-nonlinear inequalities
+        at shooting nodes (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__uphi
+
+    # convex-over-nonlinear constraints at shooting node N
+    @property
+    def lphi_e(self):
+        """:math:`\\underline{\phi}^e` - lower bound on convex-over-nonlinear inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__lphi_e
+
+    @property
+    def uphi_e(self):
+        """:math:`\\bar{\phi}^e` - upper bound on convex-over-nonlinear inequalities
+        at terminal shooting node N.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`.
+        """
+        return self.__uphi_e
+
+
+    # SLACK bounds
+    # soft bounds on x
+    @property
+    def lsbx(self):
+        """Lower bounds on slacks corresponding to soft lower bounds on x
+        at stages (1 to N-1);
+        not required - zeros by default"""
+        return self.__lsbx
+
+    @property
+    def usbx(self):
+        """Lower bounds on slacks corresponding to soft upper bounds on x
+        at stages (1 to N-1);
+        not required - zeros by default"""
+        return self.__usbx
+
+    @property
+    def idxsbx(self):
+        """Indices of soft bounds on x within the indices of bounds on x
+        at stages (1 to N-1).
+        Can be set by using :py:attr:`Jsbx`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsbx
+
+    @property
+    def Jsbx(self):
+        """:math:`J_{sbx}` - matrix coefficient for soft bounds on x
+        at stages (1 to N-1);
+        Translated internally into :py:attr:`idxsbx`."""
+        print_J_to_idx_note()
+        return self.__idxsbx
+
+    # soft bounds on u
+    @property
+    def lsbu(self):
+        """Lower bounds on slacks corresponding to soft lower bounds on u
+        at stages (0 to N-1).
+        Not required - zeros by default."""
+        return self.__lsbu
+
+    @property
+    def usbu(self):
+        """Lower bounds on slacks corresponding to soft upper bounds on u
+        at stages (0 to N-1);
+        not required - zeros by default"""
+        return self.__usbu
+
+    @property
+    def idxsbu(self):
+        """Indices of soft bounds on u within the indices of bounds on u
+        at stages (0 to N-1).
+        Can be set by using :py:attr:`Jsbu`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsbu
+
+    @property
+    def Jsbu(self):
+        """:math:`J_{sbu}` - matrix coefficient for soft bounds on u
+        at stages (0 to N-1);
+        internally translated into :py:attr:`idxsbu`"""
+        print_J_to_idx_note()
+        return self.__idxsbu
+
+    # soft bounds on x at shooting node N
+    @property
+    def lsbx_e(self):
+        """Lower bounds on slacks corresponding to soft lower bounds on x at shooting node N.
+        Not required - zeros by default"""
+        return self.__lsbx_e
+
+    @property
+    def usbx_e(self):
+        """Lower bounds on slacks corresponding to soft upper bounds on x at shooting node N.
+        Not required - zeros by default"""
+        return self.__usbx_e
+
+    @property
+    def idxsbx_e(self):
+        """Indices of soft bounds on x at shooting node N, within the indices of bounds on x at terminal shooting node N.
+        Can be set by using :py:attr:`Jsbx_e`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsbx_e
+
+    @property
+    def Jsbx_e(self):
+        """:math:`J_{sbx}^e` - matrix coefficient for soft bounds on x at terminal shooting node N.
+        Translated internally to :py:attr:`idxsbx_e`"""
+        print_J_to_idx_note()
+        return self.__idxsbx_e
+
+    # soft general linear constraints
+    @property
+    def lsg(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints
+        at stages (0 to N-1).
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`
+        """
+        return self.__lsg
+
+    @property
+    def usg(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints.
+        Not required - zeros by default"""
+        return self.__usg
+
+    @property
+    def idxsg(self):
+        """Indices of soft general linear constraints within the indices of general linear constraints.
+        Can be set by using :py:attr:`Jsg`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsg
+
+    @property
+    def Jsg(self):
+        """:math:`J_{sg}` - matrix coefficient for soft bounds on general linear constraints.
+        Translated internally to :py:attr:`idxsg`"""
+        print_J_to_idx_note()
+        return self.__idxsg
+
+    # soft nonlinear constraints
+    @property
+    def lsh(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints.
+        Not required - zeros by default"""
+        return self.__lsh
+
+    @property
+    def ush(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints.
+        Not required - zeros by default"""
+        return self.__ush
+
+    @property
+    def idxsh(self):
+        """Indices of soft nonlinear constraints within the indices of nonlinear constraints.
+        Can be set by using :py:attr:`Jbx`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsh
+
+    @property
+    def Jsh(self):
+        """:math:`J_{sh}` - matrix coefficient for soft bounds on nonlinear constraints.
+        Translated internally to :py:attr:`idxsh`"""
+        print_J_to_idx_note()
+        return self.__idxsh
+
+    # soft bounds on convex-over-nonlinear constraints
+    @property
+    def lsphi(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints.
+        Not required - zeros by default"""
+        return self.__lsphi
+
+    @property
+    def usphi(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints.
+        Not required - zeros by default"""
+        return self.__usphi
+
+    @property
+    def idxsphi(self):
+        """Indices of soft convex-over-nonlinear constraints within the indices of nonlinear constraints.
+        Can be set by using :py:attr:`Jsphi`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsphi
+
+    @property
+    def Jsphi(self):
+        """:math:`J_{s, \phi}` - matrix coefficient for soft bounds on convex-over-nonlinear constraints.
+        Translated internally into :py:attr:`idxsphi`."""
+        print_J_to_idx_note()
+        return self.__idxsphi
+
+
+    # soft bounds on general linear constraints at shooting node N
+    @property
+    def lsg_e(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for general linear constraints at shooting node N.
+        Not required - zeros by default"""
+        return self.__lsg_e
+
+    @property
+    def usg_e(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for general linear constraints at shooting node N.
+        Not required - zeros by default"""
+        return self.__usg_e
+
+    @property
+    def idxsg_e(self):
+        """Indices of soft general linear constraints at shooting node N within the indices of general linear constraints at shooting node N.
+        Can be set by using :py:attr:`Jsg_e`."""
+        return self.__idxsg_e
+
+    @property
+    def Jsg_e(self):
+        """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on general linear constraints at terminal shooting node N.
+        Translated internally to :py:attr:`idxsg_e`"""
+        print_J_to_idx_note()
+        return self.__idxsg_e
+
+
+    # soft bounds on nonlinear constraints at shooting node N
+    @property
+    def lsh_e(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for nonlinear constraints at terminal shooting node N.
+        Not required - zeros by default"""
+        return self.__lsh_e
+
+    @property
+    def ush_e(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for nonlinear constraints at terminal shooting node N.
+        Not required - zeros by default"""
+        return self.__ush_e
+
+    @property
+    def idxsh_e(self):
+        """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N.
+        Can be set by using :py:attr:`Jsh_e`."""
+        return self.__idxsh_e
+
+    @property
+    def Jsh_e(self):
+        """:math:`J_{s,h}^e` - matrix coefficient for soft bounds on nonlinear constraints at terminal shooting node N; fills :py:attr:`idxsh_e`"""
+        print_J_to_idx_note()
+        return self.__idxsh_e
+
+    # soft bounds on convex-over-nonlinear constraints at shooting node N
+    @property
+    def lsphi_e(self):
+        """Lower bounds on slacks corresponding to soft lower bounds for convex-over-nonlinear constraints at terminal shooting node N.
+        Not required - zeros by default"""
+        return self.__lsphi_e
+
+    @property
+    def usphi_e(self):
+        """Lower bounds on slacks corresponding to soft upper bounds for convex-over-nonlinear constraints at terminal shooting node N.
+        Not required - zeros by default"""
+        return self.__usphi_e
+
+    @property
+    def idxsphi_e(self):
+        """Indices of soft nonlinear constraints at shooting node N within the indices of nonlinear constraints at terminal shooting node N.
+        Can be set by using :py:attr:`Jsphi_e`.
+        Type: :code:`np.ndarray`; default: :code:`np.array([])`"""
+        return self.__idxsphi_e
+
+    @property
+    def Jsphi_e(self):
+        """:math:`J_{sh}^e` - matrix coefficient for soft bounds on convex-over-nonlinear constraints at shooting node N.
+        Translated internally to :py:attr:`idxsphi_e`"""
+        print_J_to_idx_note()
+        return self.__idxsphi_e
+
+    @property
+    def x0(self):
+        """:math:`x_0 \\in \mathbb{R}^{n_x}` - initial state --
+        Translated internally to :py:attr:`idxbx_0`, :py:attr:`lbx_0`, :py:attr:`ubx_0`, :py:attr:`idxbxe_0` """
+        print("x0 is converted to lbx_0, ubx_0, idxbx_0")
+        print("idxbx_0: ", self.__idxbx_0)
+        print("lbx_0: ", self.__lbx_0)
+        print("ubx_0: ", self.__ubx_0)
+        print("idxbxe_0: ", self.__idxbxe_0)
+        return None
+
+    # SETTERS
+    @constr_type.setter
+    def constr_type(self, constr_type):
+        constr_types = ('BGH', 'BGP')
+        if constr_type in constr_types:
+            self.__constr_type = constr_type
+        else:
+            raise Exception('Invalid constr_type value. Possible values are:\n\n' \
+                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type + '.\n\nExiting.')
+
+    @constr_type_e.setter
+    def constr_type_e(self, constr_type_e):
+        constr_types = ('BGH', 'BGP')
+        if constr_type_e in constr_types:
+            self.__constr_type_e = constr_type_e
+        else:
+            raise Exception('Invalid constr_type_e value. Possible values are:\n\n' \
+                    + ',\n'.join(constr_types) + '.\n\nYou have: ' + constr_type_e + '.\n\nExiting.')
+
+    # initial x
+    @lbx_0.setter
+    def lbx_0(self, lbx_0):
+        if type(lbx_0) == np.ndarray:
+            self.__lbx_0 = lbx_0
+        else:
+            raise Exception('Invalid lbx_0 value. Exiting.')
+
+    @ubx_0.setter
+    def ubx_0(self, ubx_0):
+        if type(ubx_0) == np.ndarray:
+            self.__ubx_0 = ubx_0
+        else:
+            raise Exception('Invalid ubx_0 value. Exiting.')
+
+    @idxbx_0.setter
+    def idxbx_0(self, idxbx_0):
+        if isinstance(idxbx_0, np.ndarray):
+            self.__idxbx_0 = idxbx_0
+        else:
+            raise Exception('Invalid idxbx_0 value. Exiting.')
+
+    @Jbx_0.setter
+    def Jbx_0(self, Jbx_0):
+        if type(Jbx_0) == np.ndarray:
+            self.__idxbx_0 = J_to_idx(Jbx_0)
+        else:
+            raise Exception('Invalid Jbx_0 value. Exiting.')
+
+    @idxbxe_0.setter
+    def idxbxe_0(self, idxbxe_0):
+        if isinstance(idxbxe_0, np.ndarray):
+            self.__idxbxe_0 = idxbxe_0
+        else:
+            raise Exception('Invalid idxbxe_0 value. Exiting.')
+
+
+    @x0.setter
+    def x0(self, x0):
+        if isinstance(x0, np.ndarray):
+            self.__lbx_0 = x0
+            self.__ubx_0 = x0
+            self.__idxbx_0 = np.arange(x0.size)
+            self.__idxbxe_0 = np.arange(x0.size)
+        else:
+            raise Exception('Invalid x0 value. Exiting.')
+
+    # bounds on x
+    @lbx.setter
+    def lbx(self, lbx):
+        if type(lbx) == np.ndarray:
+            self.__lbx = lbx
+        else:
+            raise Exception('Invalid lbx value. Exiting.')
+
+    @ubx.setter
+    def ubx(self, ubx):
+        if type(ubx) == np.ndarray:
+            self.__ubx = ubx
+        else:
+            raise Exception('Invalid ubx value. Exiting.')
+
+    @idxbx.setter
+    def idxbx(self, idxbx):
+        if type(idxbx) == np.ndarray:
+            self.__idxbx = idxbx
+        else:
+            raise Exception('Invalid idxbx value. Exiting.')
+
+    @Jbx.setter
+    def Jbx(self, Jbx):
+        if type(Jbx) == np.ndarray:
+            self.__idxbx = J_to_idx(Jbx)
+        else:
+            raise Exception('Invalid Jbx value. Exiting.')
+
+    # bounds on u
+    @lbu.setter
+    def lbu(self, lbu):
+        if type(lbu) == np.ndarray:
+            self.__lbu = lbu
+        else:
+            raise Exception('Invalid lbu value. Exiting.')
+
+    @ubu.setter
+    def ubu(self, ubu):
+        if type(ubu) == np.ndarray:
+            self.__ubu = ubu
+        else:
+            raise Exception('Invalid ubu value. Exiting.')
+
+    @idxbu.setter
+    def idxbu(self, idxbu):
+        if type(idxbu) == np.ndarray:
+            self.__idxbu = idxbu
+        else:
+            raise Exception('Invalid idxbu value. Exiting.')
+
+    @Jbu.setter
+    def Jbu(self, Jbu):
+        if type(Jbu) == np.ndarray:
+            self.__idxbu = J_to_idx(Jbu)
+        else:
+            raise Exception('Invalid Jbu value. Exiting.')
+
+    # bounds on x at shooting node N
+    @lbx_e.setter
+    def lbx_e(self, lbx_e):
+        if type(lbx_e) == np.ndarray:
+            self.__lbx_e = lbx_e
+        else:
+            raise Exception('Invalid lbx_e value. Exiting.')
+
+    @ubx_e.setter
+    def ubx_e(self, ubx_e):
+        if type(ubx_e) == np.ndarray:
+            self.__ubx_e = ubx_e
+        else:
+            raise Exception('Invalid ubx_e value. Exiting.')
+
+    @idxbx_e.setter
+    def idxbx_e(self, idxbx_e):
+        if type(idxbx_e) == np.ndarray:
+            self.__idxbx_e = idxbx_e
+        else:
+            raise Exception('Invalid idxbx_e value. Exiting.')
+
+    @Jbx_e.setter
+    def Jbx_e(self, Jbx_e):
+        if type(Jbx_e) == np.ndarray:
+            self.__idxbx_e = J_to_idx(Jbx_e)
+        else:
+            raise Exception('Invalid Jbx_e value. Exiting.')
+
+    # polytopic constraints
+    @D.setter
+    def D(self, D):
+        if isinstance(D, np.ndarray) and len(D.shape) == 2:
+            self.__D = D
+        else:
+            raise Exception('Invalid constraint D value.' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @C.setter
+    def C(self, C):
+        if isinstance(C, np.ndarray) and len(C.shape) == 2:
+            self.__C = C
+        else:
+            raise Exception('Invalid constraint C value.' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @lg.setter
+    def lg(self, lg):
+        if type(lg) == np.ndarray:
+            self.__lg = lg
+        else:
+            raise Exception('Invalid lg value. Exiting.')
+
+    @ug.setter
+    def ug(self, ug):
+        if type(ug) == np.ndarray:
+            self.__ug = ug
+        else:
+            raise Exception('Invalid ug value. Exiting.')
+
+    # polytopic constraints at shooting node N
+    @C_e.setter
+    def C_e(self, C_e):
+        if isinstance(C_e, np.ndarray) and len(C_e.shape) == 2:
+            self.__C_e = C_e
+        else:
+            raise Exception('Invalid constraint C_e value.' \
+                + 'Should be 2 dimensional numpy array. Exiting.')
+
+    @lg_e.setter
+    def lg_e(self, lg_e):
+        if type(lg_e) == np.ndarray:
+            self.__lg_e = lg_e
+        else:
+            raise Exception('Invalid lg_e value. Exiting.')
+
+    @ug_e.setter
+    def ug_e(self, ug_e):
+        if type(ug_e) == np.ndarray:
+            self.__ug_e = ug_e
+        else:
+            raise Exception('Invalid ug_e value. Exiting.')
+
+    # nonlinear constraints
+    @lh.setter
+    def lh(self, lh):
+        if type(lh) == np.ndarray:
+            self.__lh = lh
+        else:
+            raise Exception('Invalid lh value. Exiting.')
+
+    @uh.setter
+    def uh(self, uh):
+        if type(uh) == np.ndarray:
+            self.__uh = uh
+        else:
+            raise Exception('Invalid uh value. Exiting.')
+
+    # convex-over-nonlinear constraints
+    @lphi.setter
+    def lphi(self, lphi):
+        if type(lphi) == np.ndarray:
+            self.__lphi = lphi
+        else:
+            raise Exception('Invalid lphi value. Exiting.')
+
+    @uphi.setter
+    def uphi(self, uphi):
+        if type(uphi) == np.ndarray:
+            self.__uphi = uphi
+        else:
+            raise Exception('Invalid uphi value. Exiting.')
+
+    # nonlinear constraints at shooting node N
+    @lh_e.setter
+    def lh_e(self, lh_e):
+        if type(lh_e) == np.ndarray:
+            self.__lh_e = lh_e
+        else:
+            raise Exception('Invalid lh_e value. Exiting.')
+
+    @uh_e.setter
+    def uh_e(self, uh_e):
+        if type(uh_e) == np.ndarray:
+            self.__uh_e = uh_e
+        else:
+            raise Exception('Invalid uh_e value. Exiting.')
+
+    # convex-over-nonlinear constraints at shooting node N
+    @lphi_e.setter
+    def lphi_e(self, lphi_e):
+        if type(lphi_e) == np.ndarray:
+            self.__lphi_e = lphi_e
+        else:
+            raise Exception('Invalid lphi_e value. Exiting.')
+
+    @uphi_e.setter
+    def uphi_e(self, uphi_e):
+        if type(uphi_e) == np.ndarray:
+            self.__uphi_e = uphi_e
+        else:
+            raise Exception('Invalid uphi_e value. Exiting.')
+
+    # SLACK bounds
+    # soft bounds on x
+    @lsbx.setter
+    def lsbx(self, lsbx):
+        if type(lsbx) == np.ndarray:
+            self.__lsbx = lsbx
+        else:
+            raise Exception('Invalid lsbx value. Exiting.')
+
+    @usbx.setter
+    def usbx(self, usbx):
+        if type(usbx) == np.ndarray:
+            self.__usbx = usbx
+        else:
+            raise Exception('Invalid usbx value. Exiting.')
+
+    @idxsbx.setter
+    def idxsbx(self, idxsbx):
+        if type(idxsbx) == np.ndarray:
+            self.__idxsbx = idxsbx
+        else:
+            raise Exception('Invalid idxsbx value. Exiting.')
+
+    @Jsbx.setter
+    def Jsbx(self, Jsbx):
+        if isinstance(Jsbx, np.ndarray):
+            self.__idxsbx = J_to_idx_slack(Jsbx)
+        else:
+            raise Exception('Invalid Jsbx value, expected numpy array. Exiting.')
+
+    # soft bounds on u
+    @lsbu.setter
+    def lsbu(self, lsbu):
+        if type(lsbu) == np.ndarray:
+            self.__lsbu = lsbu
+        else:
+            raise Exception('Invalid lsbu value. Exiting.')
+
+    @usbu.setter
+    def usbu(self, usbu):
+        if type(usbu) == np.ndarray:
+            self.__usbu = usbu
+        else:
+            raise Exception('Invalid usbu value. Exiting.')
+
+    @idxsbu.setter
+    def idxsbu(self, idxsbu):
+        if type(idxsbu) == np.ndarray:
+            self.__idxsbu = idxsbu
+        else:
+            raise Exception('Invalid idxsbu value. Exiting.')
+
+    @Jsbu.setter
+    def Jsbu(self, Jsbu):
+        if type(Jsbu) == np.ndarray:
+            self.__idxsbu = J_to_idx_slack(Jsbu)
+        else:
+            raise Exception('Invalid Jsbu value. Exiting.')
+
+    # soft bounds on x at shooting node N
+    @lsbx_e.setter
+    def lsbx_e(self, lsbx_e):
+        if type(lsbx_e) == np.ndarray:
+            self.__lsbx_e = lsbx_e
+        else:
+            raise Exception('Invalid lsbx_e value. Exiting.')
+
+    @usbx_e.setter
+    def usbx_e(self, usbx_e):
+        if type(usbx_e) == np.ndarray:
+            self.__usbx_e = usbx_e
+        else:
+            raise Exception('Invalid usbx_e value. Exiting.')
+
+    @idxsbx_e.setter
+    def idxsbx_e(self, idxsbx_e):
+        if type(idxsbx_e) == np.ndarray:
+            self.__idxsbx_e = idxsbx_e
+        else:
+            raise Exception('Invalid idxsbx_e value. Exiting.')
+
+    @Jsbx_e.setter
+    def Jsbx_e(self, Jsbx_e):
+        if type(Jsbx_e) == np.ndarray:
+            self.__idxsbx_e = J_to_idx_slack(Jsbx_e)
+        else:
+            raise Exception('Invalid Jsbx_e value. Exiting.')
+
+
+    # soft bounds on general linear constraints
+    @lsg.setter
+    def lsg(self, lsg):
+        if isinstance(lsg, np.ndarray):
+            self.__lsg = lsg
+        else:
+            raise Exception('Invalid lsg value. Exiting.')
+
+    @usg.setter
+    def usg(self, usg):
+        if isinstance(usg, np.ndarray):
+            self.__usg = usg
+        else:
+            raise Exception('Invalid usg value. Exiting.')
+
+    @idxsg.setter
+    def idxsg(self, idxsg):
+        if isinstance(idxsg, np.ndarray):
+            self.__idxsg = idxsg
+        else:
+            raise Exception('Invalid idxsg value. Exiting.')
+
+    @Jsg.setter
+    def Jsg(self, Jsg):
+        if isinstance(Jsg, np.ndarray):
+            self.__idxsg = J_to_idx_slack(Jsg)
+        else:
+            raise Exception('Invalid Jsg value, expected numpy array. Exiting.')
+
+
+    # soft bounds on nonlinear constraints
+    @lsh.setter
+    def lsh(self, lsh):
+        if type(lsh) == np.ndarray:
+            self.__lsh = lsh
+        else:
+            raise Exception('Invalid lsh value. Exiting.')
+
+    @ush.setter
+    def ush(self, ush):
+        if type(ush) == np.ndarray:
+            self.__ush = ush
+        else:
+            raise Exception('Invalid ush value. Exiting.')
+
+    @idxsh.setter
+    def idxsh(self, idxsh):
+        if type(idxsh) == np.ndarray:
+            self.__idxsh = idxsh
+        else:
+            raise Exception('Invalid idxsh value. Exiting.')
+
+
+    @Jsh.setter
+    def Jsh(self, Jsh):
+        if isinstance(Jsh, np.ndarray):
+            self.__idxsh = J_to_idx_slack(Jsh)
+        else:
+            raise Exception('Invalid Jsh value, expected numpy array. Exiting.')
+
+    # soft bounds on convex-over-nonlinear constraints
+    @lsphi.setter
+    def lsphi(self, lsphi):
+        if type(lsphi) == np.ndarray:
+            self.__lsphi = lsphi
+        else:
+            raise Exception('Invalid lsphi value. Exiting.')
+
+    @usphi.setter
+    def usphi(self, usphi):
+        if type(usphi) == np.ndarray:
+            self.__usphi = usphi
+        else:
+            raise Exception('Invalid usphi value. Exiting.')
+
+    @idxsphi.setter
+    def idxsphi(self, idxsphi):
+        if type(idxsphi) == np.ndarray:
+            self.__idxsphi = idxsphi
+        else:
+            raise Exception('Invalid idxsphi value. Exiting.')
+
+    @Jsphi.setter
+    def Jsphi(self, Jsphi):
+        if isinstance(Jsphi, np.ndarray):
+            self.__idxsphi = J_to_idx_slack(Jsphi)
+        else:
+            raise Exception('Invalid Jsphi value, expected numpy array. Exiting.')
+
+    # soft bounds on general linear constraints at shooting node N
+    @lsg_e.setter
+    def lsg_e(self, lsg_e):
+        if isinstance(lsg_e, np.ndarray):
+            self.__lsg_e = lsg_e
+        else:
+            raise Exception('Invalid lsg_e value. Exiting.')
+
+    @usg_e.setter
+    def usg_e(self, usg_e):
+        if isinstance(usg_e, np.ndarray):
+            self.__usg_e = usg_e
+        else:
+            raise Exception('Invalid usg_e value. Exiting.')
+
+    @idxsg_e.setter
+    def idxsg_e(self, idxsg_e):
+        if isinstance(idxsg_e, np.ndarray):
+            self.__idxsg_e = idxsg_e
+        else:
+            raise Exception('Invalid idxsg_e value. Exiting.')
+
+    @Jsg_e.setter
+    def Jsg_e(self, Jsg_e):
+        if isinstance(Jsg_e, np.ndarray):
+            self.__idxsg_e = J_to_idx_slack(Jsg_e)
+        else:
+            raise Exception('Invalid Jsg_e value, expected numpy array. Exiting.')
+
+    # soft bounds on nonlinear constraints at shooting node N
+    @lsh_e.setter
+    def lsh_e(self, lsh_e):
+        if isinstance(lsh_e, np.ndarray):
+            self.__lsh_e = lsh_e
+        else:
+            raise Exception('Invalid lsh_e value. Exiting.')
+
+    @ush_e.setter
+    def ush_e(self, ush_e):
+        if isinstance(ush_e, np.ndarray):
+            self.__ush_e = ush_e
+        else:
+            raise Exception('Invalid ush_e value. Exiting.')
+
+    @idxsh_e.setter
+    def idxsh_e(self, idxsh_e):
+        if isinstance(idxsh_e, np.ndarray):
+            self.__idxsh_e = idxsh_e
+        else:
+            raise Exception('Invalid idxsh_e value. Exiting.')
+
+    @Jsh_e.setter
+    def Jsh_e(self, Jsh_e):
+        if isinstance(Jsh_e, np.ndarray):
+            self.__idxsh_e = J_to_idx_slack(Jsh_e)
+        else:
+            raise Exception('Invalid Jsh_e value, expected numpy array. Exiting.')
+
+
+    # soft bounds on convex-over-nonlinear constraints at shooting node N
+    @lsphi_e.setter
+    def lsphi_e(self, lsphi_e):
+        if isinstance(lsphi_e, np.ndarray):
+            self.__lsphi_e = lsphi_e
+        else:
+            raise Exception('Invalid lsphi_e value. Exiting.')
+
+    @usphi_e.setter
+    def usphi_e(self, usphi_e):
+        if isinstance(usphi_e, np.ndarray):
+            self.__usphi_e = usphi_e
+        else:
+            raise Exception('Invalid usphi_e value. Exiting.')
+
+    @idxsphi_e.setter
+    def idxsphi_e(self, idxsphi_e):
+        if isinstance(idxsphi_e, np.ndarray):
+            self.__idxsphi_e = idxsphi_e
+        else:
+            raise Exception('Invalid idxsphi_e value. Exiting.')
+
+    @Jsphi_e.setter
+    def Jsphi_e(self, Jsphi_e):
+        if isinstance(Jsphi_e, np.ndarray):
+            self.__idxsphi_e = J_to_idx_slack(Jsphi_e)
+        else:
+            raise Exception('Invalid Jsphi_e value. Exiting.')
+
+    def set(self, attr, value):
+        setattr(self, attr, value)
+
+
+class AcadosOcpOptions:
+    """
+    class containing the description of the solver options
+    """
+    def __init__(self):
+        self.__qp_solver        = 'PARTIAL_CONDENSING_HPIPM'  # qp solver to be used in the NLP solver
+        self.__hessian_approx   = 'GAUSS_NEWTON'              # hessian approximation
+        self.__integrator_type  = 'ERK'                       # integrator type
+        self.__tf               = None                        # prediction horizon
+        self.__nlp_solver_type  = 'SQP_RTI'                   # NLP solver
+        self.__globalization = 'FIXED_STEP'
+        self.__nlp_solver_step_length = 1.0                   # fixed Newton step length
+        self.__levenberg_marquardt = 0.0
+        self.__sim_method_num_stages  = 4                     # number of stages in the integrator
+        self.__sim_method_num_steps   = 1                     # number of steps in the integrator
+        self.__sim_method_newton_iter = 3                     # number of Newton iterations in simulation method
+        self.__sim_method_jac_reuse = 0
+        self.__qp_solver_tol_stat = None                      # QP solver stationarity tolerance
+        self.__qp_solver_tol_eq   = None                      # QP solver equality tolerance
+        self.__qp_solver_tol_ineq = None                      # QP solver inequality
+        self.__qp_solver_tol_comp = None                      # QP solver complementarity
+        self.__qp_solver_iter_max = 50                        # QP solver max iter
+        self.__qp_solver_cond_N = None                        # QP solver: new horizon after partial condensing
+        self.__qp_solver_warm_start = 0
+        self.__nlp_solver_tol_stat = 1e-6                     # NLP solver stationarity tolerance
+        self.__nlp_solver_tol_eq   = 1e-6                     # NLP solver equality tolerance
+        self.__nlp_solver_tol_ineq = 1e-6                     # NLP solver inequality
+        self.__nlp_solver_tol_comp = 1e-6                     # NLP solver complementarity
+        self.__nlp_solver_max_iter = 100                      # NLP solver maximum number of iterations
+        self.__Tsim = None                                    # automatically calculated as tf/N
+        self.__print_level = 0                                # print level
+        self.__initialize_t_slacks = 0                        # possible values: 0, 1
+        self.__model_external_shared_lib_dir   = None         # path to the the .so lib
+        self.__model_external_shared_lib_name  = None         # name of the the .so lib
+        self.__regularize_method = None
+        self.__time_steps = None
+        self.__shooting_nodes = None
+        self.__exact_hess_cost = 1
+        self.__exact_hess_dyn = 1
+        self.__exact_hess_constr = 1
+        self.__ext_cost_num_hess = 0
+        self.__alpha_min = 0.05
+        self.__alpha_reduction = 0.7
+
+
+    @property
+    def qp_solver(self):
+        """QP solver to be used in the NLP solver.
+        String in ('PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', 'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP').
+        Default: 'PARTIAL_CONDENSING_HPIPM'.
+        """
+        return self.__qp_solver
+
+    @property
+    def hessian_approx(self):
+        """Hessian approximation.
+        String in ('GAUSS_NEWTON', 'EXACT').
+        Default: 'GAUSS_NEWTON'.
+        """
+        return self.__hessian_approx
+
+    @property
+    def integrator_type(self):
+        """
+        Integrator type.
+        String in ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK').
+        Default: 'ERK'.
+        """
+        return self.__integrator_type
+
+    @property
+    def nlp_solver_type(self):
+        """NLP solver.
+        String in ('SQP', 'SQP_RTI').
+        Default: 'SQP_RTI'.
+        """
+        return self.__nlp_solver_type
+
+    @property
+    def globalization(self):
+        """Globalization type.
+        String in ('FIXED_STEP', 'MERIT_BACKTRACKING').
+        Default: 'FIXED_STEP'.
+
+        .. note:: preliminary implementation.
+        """
+        return self.__globalization
+
+    @property
+    def regularize_method(self):
+        """Regularization method for the Hessian.
+        String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`.
+
+        Default: :code:`None`.
+        """
+        return self.__regularize_method
+
+    @property
+    def nlp_solver_step_length(self):
+        """
+        Fixed Newton step length.
+        Type: float > 0.
+        Default: 1.0.
+        """
+        return self.__nlp_solver_step_length
+
+    @property
+    def levenberg_marquardt(self):
+        """
+        Factor for LM regularization.
+        Type: float >= 0
+        Default: 0.0.
+        """
+        return self.__levenberg_marquardt
+
+    @property
+    def sim_method_num_stages(self):
+        """
+        Number of stages in the integrator.
+        Type: int > 0 or ndarray of ints > 0 of shape (N,).
+        Default: 4
+        """
+        return self.__sim_method_num_stages
+
+    @property
+    def sim_method_num_steps(self):
+        """
+        Number of steps in the integrator.
+        Type: int > 0 or ndarray of ints > 0 of shape (N,).
+        Default: 1
+        """
+        return self.__sim_method_num_steps
+
+    @property
+    def sim_method_newton_iter(self):
+        """
+        Number of Newton iterations in simulation method.
+        Type: int > 0
+        Default: 3
+        """
+        return self.__sim_method_newton_iter
+
+    @property
+    def sim_method_jac_reuse(self):
+        """
+        Integer determining if jacobians are reused within integrator or ndarray of ints > 0 of shape (N,).
+        0: False (no reuse); 1: True (reuse)
+        Default: 0
+        """
+        return self.__sim_method_jac_reuse
+
+    @property
+    def qp_solver_tol_stat(self):
+        """
+        QP solver stationarity tolerance.
+        Default: :code:`None`
+        """
+        return self.__qp_solver_tol_stat
+
+    @property
+    def qp_solver_tol_eq(self):
+        """
+        QP solver equality tolerance.
+        Default: :code:`None`
+        """
+        return self.__qp_solver_tol_eq
+
+    @property
+    def qp_solver_tol_ineq(self):
+        """
+        QP solver inequality.
+        Default: :code:`None`
+        """
+        return self.__qp_solver_tol_ineq
+
+    @property
+    def qp_solver_tol_comp(self):
+        """
+        QP solver complementarity.
+        Default: :code:`None`
+        """
+        return self.__qp_solver_tol_comp
+
+    @property
+    def qp_solver_cond_N(self):
+        """QP solver: New horizon after partial condensing.
+        Set to N by default -> no condensing."""
+        return self.__qp_solver_cond_N
+
+    @property
+    def qp_solver_warm_start(self):
+        """QP solver: Warm starting.
+        0: no warm start; 1: warm start; 2: hot start."""
+        return self.__qp_solver_warm_start
+
+    @property
+    def qp_solver_iter_max(self):
+        """
+        QP solver: maximum number of iterations.
+        Type: int > 0
+        Default: 50
+        """
+        return self.__qp_solver_iter_max
+
+    @property
+    def tol(self):
+        """
+        NLP solver tolerance. Sets or gets the max of :py:attr:`nlp_solver_tol_eq`,
+        :py:attr:`nlp_solver_tol_ineq`, :py:attr:`nlp_solver_tol_comp`
+        and :py:attr:`nlp_solver_tol_stat`.
+        """
+        return max([self.__nlp_solver_tol_eq, self.__nlp_solver_tol_ineq,\
+                    self.__nlp_solver_tol_comp, self.__nlp_solver_tol_stat])
+
+    @property
+    def qp_tol(self):
+        """
+        QP solver tolerance.
+        Sets all of the following at once or gets the max of
+        :py:attr:`qp_solver_tol_eq`, :py:attr:`qp_solver_tol_ineq`,
+        :py:attr:`qp_solver_tol_comp` and
+        :py:attr:`qp_solver_tol_stat`.
+        """
+        return max([self.__qp_solver_tol_eq, self.__qp_solver_tol_ineq,\
+                    self.__qp_solver_tol_comp, self.__qp_solver_tol_stat])
+
+    @property
+    def nlp_solver_tol_stat(self):
+        """
+        NLP solver stationarity tolerance.
+        Type: float > 0
+        Default: 1e-6
+        """
+        return self.__nlp_solver_tol_stat
+
+    @property
+    def nlp_solver_tol_eq(self):
+        """NLP solver equality tolerance"""
+        return self.__nlp_solver_tol_eq
+
+    @property
+    def alpha_min(self):
+        """Minimal step size for globalization MERIT_BACKTRACKING, default: 0.05."""
+        return self.__alpha_min
+
+    @property
+    def alpha_reduction(self):
+        """Step size reduction factor for globalization MERIT_BACKTRACKING, default: 0.7."""
+        return self.__alpha_reduction
+
+    @property
+    def nlp_solver_tol_ineq(self):
+        """NLP solver inequality tolerance"""
+        return self.__nlp_solver_tol_ineq
+
+    @property
+    def nlp_solver_tol_comp(self):
+        """NLP solver complementarity tolerance"""
+        return self.__nlp_solver_tol_comp
+
+    @property
+    def nlp_solver_max_iter(self):
+        """
+        NLP solver maximum number of iterations.
+        Type: int > 0
+        Default: 100
+        """
+        return self.__nlp_solver_max_iter
+
+    @property
+    def time_steps(self):
+        """
+        Vector with time steps between the shooting nodes. Set automatically to uniform discretization if :py:attr:`N` and :py:attr:`tf` are provided.
+        Default: :code:`None`
+        """
+        return self.__time_steps
+
+    @property
+    def shooting_nodes(self):
+        """
+        Vector with the shooting nodes, time_steps will be computed from it automatically.
+        Default: :code:`None`
+        """
+        return self.__shooting_nodes
+
+    @property
+    def tf(self):
+        """
+        Prediction horizon
+        Type: float > 0
+        Default: :code:`None`
+        """
+        return self.__tf
+
+    @property
+    def Tsim(self):
+        """
+        Time horizon for one integrator step. Automatically calculated as :py:attr:`tf`/:py:attr:`N`.
+        Default: :code:`None`
+        """
+        return self.__Tsim
+
+    @property
+    def print_level(self):
+        """
+        Verbosity of printing.
+        Type: int >= 0
+        Default: 0
+        """
+        return self.__print_level
+
+    @property
+    def model_external_shared_lib_dir(self):
+        """Path to the .so lib"""
+        return self.__model_external_shared_lib_dir
+
+    @property
+    def model_external_shared_lib_name(self):
+        """Name of the .so lib"""
+        return self.__model_external_shared_lib_name
+
+    @property
+    def exact_hess_constr(self):
+        """
+        Used in case of hessian_approx == 'EXACT'.\n
+        Can be used to turn off exact hessian contributions from the constraints module.
+        """
+        return self.__exact_hess_constr
+
+    @property
+    def exact_hess_cost(self):
+        """
+        Used in case of hessian_approx == 'EXACT'.\n
+        Can be used to turn off exact hessian contributions from the cost module.
+        """
+        return self.__exact_hess_cost
+
+    @property
+    def exact_hess_dyn(self):
+        """
+        Used in case of hessian_approx == 'EXACT'.\n
+        Can be used to turn off exact hessian contributions from the dynamics module.
+        """
+        return self.__exact_hess_dyn
+
+    @property
+    def ext_cost_num_hess(self):
+        """
+        Determines if custom hessian approximation for cost contribution is used (> 0).\n
+        Or if hessian contribution is evaluated exactly using CasADi external function (=0 - default).
+        """
+        return self.__ext_cost_num_hess
+
+    @qp_solver.setter
+    def qp_solver(self, qp_solver):
+        qp_solvers = ('PARTIAL_CONDENSING_HPIPM', \
+                'FULL_CONDENSING_QPOASES', 'FULL_CONDENSING_HPIPM', \
+                'PARTIAL_CONDENSING_QPDUNES', 'PARTIAL_CONDENSING_OSQP')
+        if qp_solver in qp_solvers:
+            self.__qp_solver = qp_solver
+        else:
+            raise Exception('Invalid qp_solver value. Possible values are:\n\n' \
+                    + ',\n'.join(qp_solvers) + '.\n\nYou have: ' + qp_solver + '.\n\nExiting.')
+
+    @regularize_method.setter
+    def regularize_method(self, regularize_method):
+        regularize_methods = ('NO_REGULARIZE', 'MIRROR', 'PROJECT', \
+                                'PROJECT_REDUC_HESS', 'CONVEXIFY')
+        if regularize_method in regularize_methods:
+            self.__regularize_method = regularize_method
+        else:
+            raise Exception('Invalid regularize_method value. Possible values are:\n\n' \
+                    + ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.')
+
+    @hessian_approx.setter
+    def hessian_approx(self, hessian_approx):
+        hessian_approxs = ('GAUSS_NEWTON', 'EXACT')
+        if hessian_approx in hessian_approxs:
+            self.__hessian_approx = hessian_approx
+        else:
+            raise Exception('Invalid hessian_approx value. Possible values are:\n\n' \
+                    + ',\n'.join(hessian_approxs) + '.\n\nYou have: ' + hessian_approx + '.\n\nExiting.')
+
+    @integrator_type.setter
+    def integrator_type(self, integrator_type):
+        integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK')
+        if integrator_type in integrator_types:
+            self.__integrator_type = integrator_type
+        else:
+            raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
+                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
+
+    @tf.setter
+    def tf(self, tf):
+        self.__tf = tf
+
+    @time_steps.setter
+    def time_steps(self, time_steps):
+        self.__time_steps = time_steps
+
+    @shooting_nodes.setter
+    def shooting_nodes(self, shooting_nodes):
+        self.__shooting_nodes = shooting_nodes
+
+
+    @Tsim.setter
+    def Tsim(self, Tsim):
+        self.__Tsim = Tsim
+
+    @globalization.setter
+    def globalization(self, globalization):
+        self.__globalization = globalization
+
+    @alpha_min.setter
+    def alpha_min(self, alpha_min):
+        self.__alpha_min = alpha_min
+
+    @alpha_reduction.setter
+    def alpha_reduction(self, alpha_reduction):
+        self.__alpha_reduction = alpha_reduction
+
+    @sim_method_num_stages.setter
+    def sim_method_num_stages(self, sim_method_num_stages):
+
+        # if type(sim_method_num_stages) == int:
+        #     self.__sim_method_num_stages = sim_method_num_stages
+        # else:
+        #     raise Exception('Invalid sim_method_num_stages value. sim_method_num_stages must be an integer. Exiting.')
+
+        self.__sim_method_num_stages = sim_method_num_stages
+
+    @sim_method_num_steps.setter
+    def sim_method_num_steps(self, sim_method_num_steps):
+
+        # if type(sim_method_num_steps) == int:
+        #     self.__sim_method_num_steps = sim_method_num_steps
+        # else:
+        #     raise Exception('Invalid sim_method_num_steps value. sim_method_num_steps must be an integer. Exiting.')
+        self.__sim_method_num_steps = sim_method_num_steps
+
+
+    @sim_method_newton_iter.setter
+    def sim_method_newton_iter(self, sim_method_newton_iter):
+
+        if type(sim_method_newton_iter) == int:
+            self.__sim_method_newton_iter = sim_method_newton_iter
+        else:
+            raise Exception('Invalid sim_method_newton_iter value. sim_method_newton_iter must be an integer. Exiting.')
+
+    @sim_method_jac_reuse.setter
+    def sim_method_jac_reuse(self, sim_method_jac_reuse):
+        # if sim_method_jac_reuse in (True, False):
+        self.__sim_method_jac_reuse = sim_method_jac_reuse
+        # else:
+            # raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.')
+
+    @nlp_solver_type.setter
+    def nlp_solver_type(self, nlp_solver_type):
+        nlp_solver_types = ('SQP', 'SQP_RTI')
+        if nlp_solver_type in nlp_solver_types:
+            self.__nlp_solver_type = nlp_solver_type
+        else:
+            raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \
+                    + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.')
+
+    @nlp_solver_step_length.setter
+    def nlp_solver_step_length(self, nlp_solver_step_length):
+        if type(nlp_solver_step_length) == float and nlp_solver_step_length > 0:
+            self.__nlp_solver_step_length = nlp_solver_step_length
+        else:
+            raise Exception('Invalid nlp_solver_step_length value. nlp_solver_step_length must be a positive float. Exiting')
+
+    @levenberg_marquardt.setter
+    def levenberg_marquardt(self, levenberg_marquardt):
+        if isinstance(levenberg_marquardt, float) and levenberg_marquardt >= 0:
+            self.__levenberg_marquardt = levenberg_marquardt
+        else:
+            raise Exception('Invalid levenberg_marquardt value. levenberg_marquardt must be a positive float. Exiting')
+
+    @qp_solver_iter_max.setter
+    def qp_solver_iter_max(self, qp_solver_iter_max):
+        if isinstance(qp_solver_iter_max, int) and qp_solver_iter_max > 0:
+            self.__qp_solver_iter_max = qp_solver_iter_max
+        else:
+            raise Exception('Invalid qp_solver_iter_max value. qp_solver_iter_max must be a positive int. Exiting')
+
+    @qp_solver_cond_N.setter
+    def qp_solver_cond_N(self, qp_solver_cond_N):
+        if isinstance(qp_solver_cond_N, int) and qp_solver_cond_N > 0:
+            self.__qp_solver_cond_N = qp_solver_cond_N
+        else:
+            raise Exception('Invalid qp_solver_cond_N value. qp_solver_cond_N must be a positive int. Exiting')
+
+    @qp_solver_warm_start.setter
+    def qp_solver_warm_start(self, qp_solver_warm_start):
+        if qp_solver_warm_start in [0, 1, 2]:
+            self.__qp_solver_warm_start = qp_solver_warm_start
+        else:
+            raise Exception('Invalid qp_solver_warm_start value. qp_solver_warm_start must be 0 or 1 or 2. Exiting')
+
+    @qp_tol.setter
+    def qp_tol(self, qp_tol):
+        if isinstance(qp_tol, float) and qp_tol > 0:
+            self.__qp_solver_tol_eq = qp_tol
+            self.__qp_solver_tol_ineq = qp_tol
+            self.__qp_solver_tol_stat = qp_tol
+            self.__qp_solver_tol_comp = qp_tol
+        else:
+            raise Exception('Invalid qp_tol value. qp_tol must be a positive float. Exiting')
+
+    @qp_solver_tol_stat.setter
+    def qp_solver_tol_stat(self, qp_solver_tol_stat):
+        if isinstance(qp_solver_tol_stat, float) and qp_solver_tol_stat > 0:
+            self.__qp_solver_tol_stat = qp_solver_tol_stat
+        else:
+            raise Exception('Invalid qp_solver_tol_stat value. qp_solver_tol_stat must be a positive float. Exiting')
+
+    @qp_solver_tol_eq.setter
+    def qp_solver_tol_eq(self, qp_solver_tol_eq):
+        if isinstance(qp_solver_tol_eq, float) and qp_solver_tol_eq > 0:
+            self.__qp_solver_tol_eq = qp_solver_tol_eq
+        else:
+            raise Exception('Invalid qp_solver_tol_eq value. qp_solver_tol_eq must be a positive float. Exiting')
+
+    @qp_solver_tol_ineq.setter
+    def qp_solver_tol_ineq(self, qp_solver_tol_ineq):
+        if isinstance(qp_solver_tol_ineq, float) and qp_solver_tol_ineq > 0:
+            self.__qp_solver_tol_ineq = qp_solver_tol_ineq
+        else:
+            raise Exception('Invalid qp_solver_tol_ineq value. qp_solver_tol_ineq must be a positive float. Exiting')
+
+    @qp_solver_tol_comp.setter
+    def qp_solver_tol_comp(self, qp_solver_tol_comp):
+        if isinstance(qp_solver_tol_comp, float) and qp_solver_tol_comp > 0:
+            self.__qp_solver_tol_comp = qp_solver_tol_comp
+        else:
+            raise Exception('Invalid qp_solver_tol_comp value. qp_solver_tol_comp must be a positive float. Exiting')
+
+    @tol.setter
+    def tol(self, tol):
+        if isinstance(tol, float) and tol > 0:
+            self.__nlp_solver_tol_eq = tol
+            self.__nlp_solver_tol_ineq = tol
+            self.__nlp_solver_tol_stat = tol
+            self.__nlp_solver_tol_comp = tol
+        else:
+            raise Exception('Invalid tol value. tol must be a positive float. Exiting')
+
+    @nlp_solver_tol_stat.setter
+    def nlp_solver_tol_stat(self, nlp_solver_tol_stat):
+        if isinstance(nlp_solver_tol_stat, float) and nlp_solver_tol_stat > 0:
+            self.__nlp_solver_tol_stat = nlp_solver_tol_stat
+        else:
+            raise Exception('Invalid nlp_solver_tol_stat value. nlp_solver_tol_stat must be a positive float. Exiting')
+
+    @nlp_solver_tol_eq.setter
+    def nlp_solver_tol_eq(self, nlp_solver_tol_eq):
+        if isinstance(nlp_solver_tol_eq, float) and nlp_solver_tol_eq > 0:
+            self.__nlp_solver_tol_eq = nlp_solver_tol_eq
+        else:
+            raise Exception('Invalid nlp_solver_tol_eq value. nlp_solver_tol_eq must be a positive float. Exiting')
+
+    @nlp_solver_tol_ineq.setter
+    def nlp_solver_tol_ineq(self, nlp_solver_tol_ineq):
+        if isinstance(nlp_solver_tol_ineq, float) and nlp_solver_tol_ineq > 0:
+            self.__nlp_solver_tol_ineq = nlp_solver_tol_ineq
+        else:
+            raise Exception('Invalid nlp_solver_tol_ineq value. nlp_solver_tol_ineq must be a positive float. Exiting')
+
+    @nlp_solver_tol_comp.setter
+    def nlp_solver_tol_comp(self, nlp_solver_tol_comp):
+        if isinstance(nlp_solver_tol_comp, float) and nlp_solver_tol_comp > 0:
+            self.__nlp_solver_tol_comp = nlp_solver_tol_comp
+        else:
+            raise Exception('Invalid nlp_solver_tol_comp value. nlp_solver_tol_comp must be a positive float. Exiting')
+
+    @nlp_solver_max_iter.setter
+    def nlp_solver_max_iter(self, nlp_solver_max_iter):
+
+        if type(nlp_solver_max_iter) == int and nlp_solver_max_iter > 0:
+            self.__nlp_solver_max_iter = nlp_solver_max_iter
+        else:
+            raise Exception('Invalid nlp_solver_max_iter value. nlp_solver_max_iter must be a positive int. Exiting')
+
+    @print_level.setter
+    def print_level(self, print_level):
+        if type(print_level) == int and print_level >= 0:
+            self.__print_level = print_level
+        else:
+            raise Exception('Invalid print_level value. print_level takes one of the values >=0. Exiting')
+
+    @model_external_shared_lib_dir.setter
+    def model_external_shared_lib_dir(self, model_external_shared_lib_dir):
+        if type(model_external_shared_lib_dir) == str :
+            self.__model_external_shared_lib_dir = model_external_shared_lib_dir
+        else:
+            raise Exception('Invalid model_external_shared_lib_dir value. Str expected.' \
+            + '.\n\nYou have: ' + type(model_external_shared_lib_dir) + '.\n\nExiting.')
+
+    @model_external_shared_lib_name.setter
+    def model_external_shared_lib_name(self, model_external_shared_lib_name):
+        if type(model_external_shared_lib_name) == str :
+            if model_external_shared_lib_name[-3:] == '.so' : 
+                raise Exception('Invalid model_external_shared_lib_name value. Remove the .so extension.' \
+            + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
+            else :
+                self.__model_external_shared_lib_name = model_external_shared_lib_name
+        else:
+            raise Exception('Invalid model_external_shared_lib_name value. Str expected.' \
+            + '.\n\nYou have: ' + type(model_external_shared_lib_name) + '.\n\nExiting.')
+
+    @exact_hess_constr.setter
+    def exact_hess_constr(self, exact_hess_constr):
+        if exact_hess_constr in [0, 1]:
+            self.__exact_hess_constr = exact_hess_constr
+        else:
+            raise Exception('Invalid exact_hess_constr value. exact_hess_constr takes one of the values 0, 1. Exiting')
+
+    @exact_hess_cost.setter
+    def exact_hess_cost(self, exact_hess_cost):
+        if exact_hess_cost in [0, 1]:
+            self.__exact_hess_cost = exact_hess_cost
+        else:
+            raise Exception('Invalid exact_hess_cost value. exact_hess_cost takes one of the values 0, 1. Exiting')
+
+    @exact_hess_dyn.setter
+    def exact_hess_dyn(self, exact_hess_dyn):
+        if exact_hess_dyn in [0, 1]:
+            self.__exact_hess_dyn = exact_hess_dyn
+        else:
+            raise Exception('Invalid exact_hess_dyn value. exact_hess_dyn takes one of the values 0, 1. Exiting')
+
+    @ext_cost_num_hess.setter
+    def ext_cost_num_hess(self, ext_cost_num_hess):
+        if ext_cost_num_hess in [0, 1]:
+            self.__ext_cost_num_hess = ext_cost_num_hess
+        else:
+            raise Exception('Invalid ext_cost_num_hess value. ext_cost_num_hess takes one of the values 0, 1. Exiting')
+
+    def set(self, attr, value):
+        setattr(self, attr, value)
+
+
+class AcadosOcp:
+    """
+    Class containing the full description of the optimal control problem.
+    This object can be used to create an :py:class:`acados_template.acados_ocp_solver.AcadosOcpSolver`.
+
+    The class has the following properties that can be modified to formulate a specific OCP, see below:
+
+        - :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosOcpDims`
+        - :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel`
+        - :py:attr:`cost` of type :py:class:`acados_template.acados_ocp.AcadosOcpCost`
+        - :py:attr:`constraints` of type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`
+        - :py:attr:`solver_options` of type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`
+
+        - :py:attr:`acados_include_path` (set automatically)
+        - :py:attr:`acados_lib_path` (set automatically)
+        - :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
+    """
+    def __init__(self, acados_path=''):
+        """
+        Keyword arguments:
+        acados_path -- path of your acados installation
+        """
+        if acados_path == '':
+            acados_path = get_acados_path()
+
+        self.dims = AcadosOcpDims()
+        """Dimension definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpDims`"""
+        self.model = AcadosModel()
+        """Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`"""
+        self.cost = AcadosOcpCost()
+        """Cost definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpCost`"""
+        self.constraints = AcadosOcpConstraints()
+        """Constraints definitions, type :py:class:`acados_template.acados_ocp.AcadosOcpConstraints`"""
+        self.solver_options = AcadosOcpOptions()
+        """Solver Options, type :py:class:`acados_template.acados_ocp.AcadosOcpOptions`"""
+		
+        self.acados_include_path = f'{acados_path}/include'
+        """Path to acados include directory, type: string"""
+        self.acados_lib_path = f'{acados_path}/lib'
+        """Path to where acados library is located, type: string"""
+
+        self.__parameter_values = np.array([])
+        self.__problem_class = 'OCP'
+
+        self.code_export_directory = 'c_generated_code'
+        """Path to where code will be exported. Default: `c_generated_code`."""
+
+    @property
+    def parameter_values(self):
+        """:math:`p` - initial values for parameter - can be updated stagewise"""
+        return self.__parameter_values
+
+    @parameter_values.setter
+    def parameter_values(self, parameter_values):
+        if isinstance(parameter_values, np.ndarray):
+            self.__parameter_values = parameter_values
+        else:
+            raise Exception('Invalid parameter_values value. ' +
+                            f'Expected numpy array, got {type(parameter_values)}.')
+
+    def set(self, attr, value):
+        # tokenize string
+        tokens = attr.split('_', 1)
+        if len(tokens) > 1:
+            setter_to_call = getattr(getattr(self, tokens[0]), 'set')
+        else:
+            setter_to_call = getattr(self, 'set')
+
+        setter_to_call(tokens[1], value)
+
+        return
diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py
new file mode 100644
index 000000000..4e80e1205
--- /dev/null
+++ b/pyextra/acados_template/acados_ocp_solver.py
@@ -0,0 +1,1388 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import sys
+import os
+import json
+import numpy as np
+from datetime import datetime
+import ctypes
+from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref
+
+from copy import deepcopy
+
+from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
+from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
+from .generate_c_code_gnsf import generate_c_code_gnsf
+from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
+from .generate_c_code_constraint import generate_c_code_constraint
+from .generate_c_code_nls_cost import generate_c_code_nls_cost
+from .generate_c_code_external_cost import generate_c_code_external_cost
+from .acados_ocp import AcadosOcp
+from .acados_model import acados_model_strip_casadi_symbolics
+from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
+     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
+     set_up_imported_gnsf_model, get_acados_path
+
+
+def make_ocp_dims_consistent(acados_ocp):
+    dims = acados_ocp.dims
+    cost = acados_ocp.cost
+    constraints = acados_ocp.constraints
+    model = acados_ocp.model
+    opts = acados_ocp.solver_options
+
+    # nx
+    if is_column(model.x):
+        dims.nx = casadi_length(model.x)
+    else:
+        raise Exception('model.x should be column vector!')
+
+    # nu
+    if is_empty(model.u):
+        dims.nu = 0
+    else:
+        dims.nu = casadi_length(model.u)
+
+    # nz
+    if is_empty(model.z):
+        dims.nz = 0
+    else:
+        dims.nz = casadi_length(model.z)
+
+    # np
+    if is_empty(model.p):
+        dims.np = 0
+    else:
+        dims.np = casadi_length(model.p)
+    if acados_ocp.parameter_values.shape[0] != dims.np:
+        raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \
+            f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n')
+
+    # cost
+    # initial stage - if not set, copy fields from path constraints
+    if cost.cost_type_0 is None:
+        cost.cost_type_0 = cost.cost_type
+        cost.W_0 = cost.W
+        cost.Vx_0 = cost.Vx
+        cost.Vu_0 = cost.Vu
+        cost.Vz_0 = cost.Vz
+        cost.yref_0 = cost.yref
+        cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type
+        model.cost_y_expr_0 = model.cost_y_expr
+        model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost
+
+    if cost.cost_type_0 == 'LINEAR_LS':
+        ny_0 = cost.W_0.shape[0]
+        if cost.Vx_0.shape[0] != ny_0 or cost.Vu_0.shape[0] != ny_0:
+            raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0.' + \
+                            f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}]\n')
+        if dims.nz != 0 and cost.Vz_0.shape[0] != ny_0:
+            raise Exception('inconsistent dimension ny_0, regarding W_0, Vx_0, Vu_0, Vz_0.' + \
+                            f'\nGot W_0[{cost.W_0.shape}], Vx_0[{cost.Vx_0.shape}], Vu_0[{cost.Vu_0.shape}], Vz_0[{cost.Vz_0.shape}]\n')
+        if cost.Vx_0.shape[1] != dims.nx and ny_0 != 0:
+            raise Exception('inconsistent dimension: Vx_0 should have nx columns.')
+        if cost.Vu_0.shape[1] != dims.nu and ny_0 != 0:
+            raise Exception('inconsistent dimension: Vu_0 should have nu columns.')
+        if cost.yref_0.shape[0] != ny_0:
+            raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \
+                            f'\nGot W_0[{cost.W_0.shape}], yref_0[{cost.yref_0.shape}]\n')
+        dims.ny_0 = ny_0
+
+    elif cost.cost_type_0 == 'NONLINEAR_LS':
+        ny_0 = cost.W_0.shape[0]
+        if is_empty(model.cost_y_expr_0) and ny_0 != 0:
+            raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.')
+        elif casadi_length(model.cost_y_expr_0) != ny_0:
+            raise Exception('inconsistent dimension ny_0: regarding W_0, cost_y_expr.')
+        if cost.yref_0.shape[0] != ny_0:
+            raise Exception('inconsistent dimension: regarding W_0, yref_0.' + \
+                            f'\nGot W_0[{cost.W.shape}], yref_0[{cost.yref_0.shape}]\n')
+        dims.ny_0 = ny_0
+
+    # path
+    if cost.cost_type == 'LINEAR_LS':
+        ny = cost.W.shape[0]
+        if cost.Vx.shape[0] != ny or cost.Vu.shape[0] != ny:
+            raise Exception('inconsistent dimension ny, regarding W, Vx, Vu.' + \
+                            f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}]\n')
+        if dims.nz != 0 and cost.Vz.shape[0] != ny:
+            raise Exception('inconsistent dimension ny, regarding W, Vx, Vu, Vz.' + \
+                            f'\nGot W[{cost.W.shape}], Vx[{cost.Vx.shape}], Vu[{cost.Vu.shape}], Vz[{cost.Vz.shape}]\n')
+        if cost.Vx.shape[1] != dims.nx and ny != 0:
+            raise Exception('inconsistent dimension: Vx should have nx columns.')
+        if cost.Vu.shape[1] != dims.nu and ny != 0:
+            raise Exception('inconsistent dimension: Vu should have nu columns.')
+        if cost.yref.shape[0] != ny:
+            raise Exception('inconsistent dimension: regarding W, yref.' + \
+                            f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n')
+        dims.ny = ny
+
+    elif cost.cost_type == 'NONLINEAR_LS':
+        ny = cost.W.shape[0]
+        if is_empty(model.cost_y_expr) and ny != 0:
+            raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.')
+        elif casadi_length(model.cost_y_expr) != ny:
+            raise Exception('inconsistent dimension ny: regarding W, cost_y_expr.')
+        if cost.yref.shape[0] != ny:
+            raise Exception('inconsistent dimension: regarding W, yref.' + \
+                            f'\nGot W[{cost.W.shape}], yref[{cost.yref.shape}]\n')
+        dims.ny = ny
+
+    # terminal
+    if cost.cost_type_e == 'LINEAR_LS':
+        ny_e = cost.W_e.shape[0]
+        if cost.Vx_e.shape[0] != ny_e:
+            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \
+                f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]')
+        if cost.Vx_e.shape[1] != dims.nx and ny_e != 0:
+            raise Exception('inconsistent dimension: Vx_e should have nx columns.')
+        if cost.yref_e.shape[0] != ny_e:
+            raise Exception('inconsistent dimension: regarding W_e, yref_e.')
+        dims.ny_e = ny_e
+
+    elif cost.cost_type_e == 'NONLINEAR_LS':
+        ny_e = cost.W_e.shape[0]
+        if is_empty(model.cost_y_expr_e) and ny_e != 0:
+            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.')
+        elif casadi_length(model.cost_y_expr_e) != ny_e:
+            raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.')
+        if cost.yref_e.shape[0] != ny_e:
+            raise Exception('inconsistent dimension: regarding W_e, yref_e.')
+        dims.ny_e = ny_e
+
+
+    ## constraints
+    # initial
+    if (constraints.lbx_0 == [] and constraints.ubx_0 == []):
+        dims.nbx_0 = 0
+    else:
+        this_shape = constraints.lbx_0.shape
+        other_shape = constraints.ubx_0.shape
+        if not this_shape == other_shape:
+            raise Exception('lbx_0, ubx_0 have different shapes!')
+        if not is_column(constraints.lbx_0):
+            raise Exception('lbx_0, ubx_0 must be column vectors!')
+        dims.nbx_0 = constraints.lbx_0.size
+
+    if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \
+        and dims.nbxe_0 is None \
+        and (constraints.idxbxe_0.shape == constraints.idxbx_0.shape)\
+            and all(constraints.idxbxe_0 == constraints.idxbx_0):
+        # case: x0 was set: nbx0 are all equlities.
+        dims.nbxe_0 = dims.nbx_0
+    elif dims.nbxe_0 is None:
+        # case: x0 was not set -> dont assume nbx0 to be equality constraints.
+        dims.nbxe_0 = 0
+
+    # path
+    nbx = constraints.idxbx.shape[0]
+    if constraints.ubx.shape[0] != nbx or constraints.lbx.shape[0] != nbx:
+        raise Exception('inconsistent dimension nbx, regarding idxbx, ubx, lbx.')
+    else:
+        dims.nbx = nbx
+
+    nbu = constraints.idxbu.shape[0]
+    if constraints.ubu.shape[0] != nbu or constraints.lbu.shape[0] != nbu:
+        raise Exception('inconsistent dimension nbu, regarding idxbu, ubu, lbu.')
+    else:
+        dims.nbu = nbu
+
+    ng = constraints.lg.shape[0]
+    if constraints.ug.shape[0] != ng or constraints.C.shape[0] != ng \
+       or constraints.D.shape[0] != ng:
+        raise Exception('inconsistent dimension ng, regarding lg, ug, C, D.')
+    else:
+        dims.ng = ng
+
+    if not is_empty(model.con_h_expr):
+        nh = casadi_length(model.con_h_expr)
+    else:
+        nh = 0
+
+    if constraints.uh.shape[0] != nh or constraints.lh.shape[0] != nh:
+        raise Exception('inconsistent dimension nh, regarding lh, uh, con_h_expr.')
+    else:
+        dims.nh = nh
+
+    if is_empty(model.con_phi_expr):
+        dims.nphi = 0
+        dims.nr = 0
+    else:
+        dims.nphi = casadi_length(model.con_phi_expr)
+        if is_empty(model.con_r_expr):
+            raise Exception('convex over nonlinear constraints: con_r_expr but con_phi_expr is nonempty')
+        else:
+            dims.nr = casadi_length(model.con_r_expr)
+
+    # terminal
+    nbx_e = constraints.idxbx_e.shape[0]
+    if constraints.ubx_e.shape[0] != nbx_e or constraints.lbx_e.shape[0] != nbx_e:
+        raise Exception('inconsistent dimension nbx_e, regarding idxbx_e, ubx_e, lbx_e.')
+    else:
+        dims.nbx_e = nbx_e
+
+    ng_e = constraints.lg_e.shape[0]
+    if constraints.ug_e.shape[0] != ng_e or constraints.C_e.shape[0] != ng_e:
+        raise Exception('inconsistent dimension ng_e, regarding_e lg_e, ug_e, C_e.')
+    else:
+        dims.ng_e = ng_e
+
+    if not is_empty(model.con_h_expr_e):
+        nh_e = casadi_length(model.con_h_expr_e)
+    else:
+        nh_e = 0
+
+    if constraints.uh_e.shape[0] != nh_e or constraints.lh_e.shape[0] != nh_e:
+        raise Exception('inconsistent dimension nh_e, regarding lh_e, uh_e, con_h_expr_e.')
+    else:
+        dims.nh_e = nh_e
+
+    if is_empty(model.con_phi_expr_e):
+        dims.nphi_e = 0
+        dims.nr_e = 0
+    else:
+        dims.nphi_e = casadi_length(model.con_phi_expr_e)
+        if is_empty(model.con_r_expr_e):
+            raise Exception('convex over nonlinear constraints: con_r_expr_e but con_phi_expr_e is nonempty')
+        else:
+            dims.nr_e = casadi_length(model.con_r_expr_e)
+
+    # Slack dimensions
+    nsbx = constraints.idxsbx.shape[0]
+    if is_empty(constraints.lsbx):
+        constraints.lsbx = np.zeros((nsbx,))
+    elif constraints.lsbx.shape[0] != nsbx:
+        raise Exception('inconsistent dimension nsbx, regarding idxsbx, lsbx.')
+    if is_empty(constraints.usbx):
+        constraints.usbx = np.zeros((nsbx,))
+    elif constraints.usbx.shape[0] != nsbx:
+        raise Exception('inconsistent dimension nsbx, regarding idxsbx, usbx.')
+    dims.nsbx = nsbx
+
+    nsbu = constraints.idxsbu.shape[0]
+    if is_empty(constraints.lsbu):
+        constraints.lsbu = np.zeros((nsbu,))
+    elif constraints.lsbu.shape[0] != nsbu:
+        raise Exception('inconsistent dimension nsbu, regarding idxsbu, lsbu.')
+    if is_empty(constraints.usbu):
+        constraints.usbu = np.zeros((nsbu,))
+    elif constraints.usbu.shape[0] != nsbu:
+        raise Exception('inconsistent dimension nsbu, regarding idxsbu, usbu.')
+    dims.nsbu = nsbu
+
+    nsh = constraints.idxsh.shape[0]
+    if is_empty(constraints.lsh):
+        constraints.lsh = np.zeros((nsh,))
+    elif constraints.lsh.shape[0] != nsh:
+        raise Exception('inconsistent dimension nsh, regarding idxsh, lsh.')
+    if is_empty(constraints.ush):
+        constraints.ush = np.zeros((nsh,))
+    elif constraints.ush.shape[0] != nsh:
+        raise Exception('inconsistent dimension nsh, regarding idxsh, ush.')
+    dims.nsh = nsh
+
+    nsphi = constraints.idxsphi.shape[0]
+    if is_empty(constraints.lsphi):
+        constraints.lsphi = np.zeros((nsphi,))
+    elif constraints.lsphi.shape[0] != nsphi:
+        raise Exception('inconsistent dimension nsphi, regarding idxsphi, lsphi.')
+    if is_empty(constraints.usphi):
+        constraints.usphi = np.zeros((nsphi,))
+    elif constraints.usphi.shape[0] != nsphi:
+        raise Exception('inconsistent dimension nsphi, regarding idxsphi, usphi.')
+    dims.nsphi = nsphi
+
+    nsg = constraints.idxsg.shape[0]
+    if is_empty(constraints.lsg):
+        constraints.lsg = np.zeros((nsg,))
+    elif constraints.lsg.shape[0] != nsg:
+        raise Exception('inconsistent dimension nsg, regarding idxsg, lsg.')
+    if is_empty(constraints.usg):
+        constraints.usg = np.zeros((nsg,))
+    elif constraints.usg.shape[0] != nsg:
+        raise Exception('inconsistent dimension nsg, regarding idxsg, usg.')
+    dims.nsg = nsg
+
+    ns = nsbx + nsbu + nsh + nsg + nsphi
+    wrong_field = ""
+    if cost.Zl.shape[0] != ns:
+        wrong_field = "Zl"
+        dim = cost.Zl.shape[0]
+    elif cost.Zu.shape[0] != ns:
+        wrong_field = "Zu"
+        dim = cost.Zu.shape[0]
+    elif cost.zl.shape[0] != ns:
+        wrong_field = "zl"
+        dim = cost.zl.shape[0]
+    elif cost.zu.shape[0] != ns:
+        wrong_field = "zu"
+        dim = cost.zu.shape[0]
+
+    if wrong_field != "":
+        raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\
+            + f'Detected ns = {ns} = nsbx + nsbu + nsg + nsh + nsphi.\n\t'\
+            + f'With nsbx = {nsbx}, nsbu = {nsbu}, nsg = {nsg}, nsh = {nsh}, nsphi = {nsphi}')
+
+    dims.ns = ns
+
+    nsbx_e = constraints.idxsbx_e.shape[0]
+    if is_empty(constraints.lsbx_e):
+        constraints.lsbx_e = np.zeros((nsbx_e,))
+    elif constraints.lsbx_e.shape[0] != nsbx_e:
+        raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, lsbx_e.')
+    if is_empty(constraints.usbx_e):
+        constraints.usbx_e = np.zeros((nsbx_e,))
+    elif constraints.usbx_e.shape[0] != nsbx_e:
+        raise Exception('inconsistent dimension nsbx_e, regarding idxsbx_e, usbx_e.')
+    dims.nsbx_e = nsbx_e
+
+    nsh_e = constraints.idxsh_e.shape[0]
+    if is_empty(constraints.lsh_e):
+        constraints.lsh_e = np.zeros((nsh_e,))
+    elif constraints.lsh_e.shape[0] != nsh_e:
+        raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, lsh_e.')
+    if is_empty(constraints.ush_e):
+        constraints.ush_e = np.zeros((nsh_e,))
+    elif constraints.ush_e.shape[0] != nsh_e:
+        raise Exception('inconsistent dimension nsh_e, regarding idxsh_e, ush_e.')
+    dims.nsh_e = nsh_e
+
+    nsg_e = constraints.idxsg_e.shape[0]
+    if is_empty(constraints.lsg_e):
+        constraints.lsg_e = np.zeros((nsg_e,))
+    elif constraints.lsg_e.shape[0] != nsg_e:
+        raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, lsg_e.')
+    if is_empty(constraints.usg_e):
+        constraints.usg_e = np.zeros((nsg_e,))
+    elif constraints.usg_e.shape[0] != nsg_e:
+        raise Exception('inconsistent dimension nsg_e, regarding idxsg_e, usg_e.')
+    dims.nsg_e = nsg_e
+
+    nsphi_e = constraints.idxsphi_e.shape[0]
+    if is_empty(constraints.lsphi_e):
+        constraints.lsphi_e = np.zeros((nsphi_e,))
+    elif constraints.lsphi_e.shape[0] != nsphi_e:
+        raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, lsphi_e.')
+    if is_empty(constraints.usphi_e):
+        constraints.usphi_e = np.zeros((nsphi_e,))
+    elif constraints.usphi_e.shape[0] != nsphi_e:
+        raise Exception('inconsistent dimension nsphi_e, regarding idxsphi_e, usphi_e.')
+    dims.nsphi_e = nsphi_e
+
+    # terminal
+    ns_e = nsbx_e + nsh_e + nsg_e + nsphi_e
+    wrong_field = ""
+    if cost.Zl_e.shape[0] != ns_e:
+        wrong_field = "Zl_e"
+        dim = cost.Zl_e.shape[0]
+    elif cost.Zu_e.shape[0] != ns_e:
+        wrong_field = "Zu_e"
+        dim = cost.Zu_e.shape[0]
+    elif cost.zl_e.shape[0] != ns_e:
+        wrong_field = "zl_e"
+        dim = cost.zl_e.shape[0]
+    elif cost.zu_e.shape[0] != ns_e:
+        wrong_field = "zu_e"
+        dim = cost.zu_e.shape[0]
+
+    if wrong_field != "":
+        raise Exception(f'Inconsistent size for field {wrong_field}, with dimension {dim}, \n\t'\
+            + f'Detected ns_e = {ns_e} = nsbx_e + nsg_e + nsh_e + nsphi_e.\n\t'\
+            + f'With nsbx_e = {nsbx_e}, nsg_e = {nsg_e}, nsh_e = {nsh_e}, nsphi_e = {nsphi_e}')
+
+    dims.ns_e = ns_e
+
+    # discretization
+    if is_empty(opts.time_steps) and is_empty(opts.shooting_nodes):
+        # uniform discretization
+        opts.time_steps = opts.tf / dims.N * np.ones((dims.N,))
+
+    elif not is_empty(opts.shooting_nodes):
+        if np.shape(opts.shooting_nodes)[0] != dims.N+1:
+            raise Exception('inconsistent dimension N, regarding shooting_nodes.')
+
+        time_steps = np.zeros((dims.N,))
+        for i in range(dims.N):
+            time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i]
+        opts.time_steps = time_steps
+
+    elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)):
+        Exception('Please provide either time_steps or shooting_nodes for nonuniform discretization')
+
+    tf = np.sum(opts.time_steps)
+    if (tf - opts.tf) / tf > 1e-15:
+        raise Exception(f'Inconsistent discretization: {opts.tf}'\
+            f' = tf != sum(opts.time_steps) = {tf}.')
+
+    # num_steps
+    if isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == 1:
+        opts.sim_method_num_steps = opts.sim_method_num_steps.item()
+
+    if isinstance(opts.sim_method_num_steps, (int, float)) and opts.sim_method_num_steps % 1 == 0:
+        opts.sim_method_num_steps = opts.sim_method_num_steps * np.ones((dims.N,), dtype=np.int64)
+    elif isinstance(opts.sim_method_num_steps, np.ndarray) and opts.sim_method_num_steps.size == dims.N \
+           and np.all(np.equal(np.mod(opts.sim_method_num_steps, 1), 0)):
+        opts.sim_method_num_steps = np.reshape(opts.sim_method_num_steps, (dims.N,)).astype(np.int64)
+    else:
+        raise Exception("Wrong value for sim_method_num_steps. Should be either int or array of ints of shape (N,).")
+
+    # num_stages
+    if isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == 1:
+        opts.sim_method_num_stages = opts.sim_method_num_stages.item()
+
+    if isinstance(opts.sim_method_num_stages, (int, float)) and opts.sim_method_num_stages % 1 == 0:
+        opts.sim_method_num_stages = opts.sim_method_num_stages * np.ones((dims.N,), dtype=np.int64)
+    elif isinstance(opts.sim_method_num_stages, np.ndarray) and opts.sim_method_num_stages.size == dims.N \
+           and np.all(np.equal(np.mod(opts.sim_method_num_stages, 1), 0)):
+        opts.sim_method_num_stages = np.reshape(opts.sim_method_num_stages, (dims.N,)).astype(np.int64)
+    else:
+        raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).")
+
+    # jac_reuse
+    if isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == 1:
+        opts.sim_method_jac_reuse = opts.sim_method_jac_reuse.item()
+
+    if isinstance(opts.sim_method_jac_reuse, (int, float)) and opts.sim_method_jac_reuse % 1 == 0:
+        opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * np.ones((dims.N,), dtype=np.int64)
+    elif isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == dims.N \
+           and np.all(np.equal(np.mod(opts.sim_method_jac_reuse, 1), 0)):
+        opts.sim_method_jac_reuse = np.reshape(opts.sim_method_jac_reuse, (dims.N,)).astype(np.int64)
+    else:
+        raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).")
+
+
+
+def get_ocp_nlp_layout():
+    current_module = sys.modules[__name__]
+    acados_path = os.path.dirname(current_module.__file__)
+    with open(acados_path + '/acados_layout.json', 'r') as f:
+        ocp_nlp_layout = json.load(f)
+    return ocp_nlp_layout
+
+
+def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'):
+    # Load acados_ocp_nlp structure description
+    ocp_layout = get_ocp_nlp_layout()
+
+    # Copy input ocp object dictionary
+    ocp_nlp_dict = dict(deepcopy(acados_ocp).__dict__)
+    # TODO: maybe make one function with formatting
+
+    for acados_struct, v in ocp_layout.items():
+        # skip non dict attributes
+        if not isinstance(v, dict):
+            continue
+        #  setattr(ocp_nlp, acados_struct, dict(getattr(acados_ocp, acados_struct).__dict__))
+        # Copy ocp object attributes dictionaries
+        ocp_nlp_dict[acados_struct]=dict(getattr(acados_ocp, acados_struct).__dict__)
+
+    ocp_nlp_dict = format_class_dict(ocp_nlp_dict)
+
+    # strip symbolics
+    ocp_nlp_dict['model'] = acados_model_strip_casadi_symbolics(ocp_nlp_dict['model'])
+
+    # strip shooting_nodes
+    ocp_nlp_dict['solver_options'].pop('shooting_nodes', None)
+
+    dims_dict = acados_class2dict(acados_ocp.dims)
+
+    ocp_check_against_layout(ocp_nlp_dict, dims_dict)
+
+    # add simulink options
+    ocp_nlp_dict['simulink_opts'] = simulink_opts
+
+    with open(json_file, 'w') as f:
+        json.dump(ocp_nlp_dict, f, default=np_array_to_list, indent=4, sort_keys=True)
+
+
+
+def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'):
+    # Load acados_ocp_nlp structure description
+    ocp_layout = get_ocp_nlp_layout()
+
+    with open(json_file, 'r') as f:
+        ocp_nlp_json = json.load(f)
+
+    ocp_nlp_dict = json2dict(ocp_nlp_json, ocp_nlp_json['dims'])
+
+    # Instantiate AcadosOcp object
+    acados_ocp = AcadosOcp()
+
+    # load class dict
+    acados_ocp.__dict__ = ocp_nlp_dict
+
+    # load class attributes dict, dims, constraints, etc
+    for acados_struct, v in ocp_layout.items():
+        # skip non dict attributes
+        if not isinstance(v, dict):
+            continue
+        acados_attribute = getattr(acados_ocp, acados_struct)
+        acados_attribute.__dict__ = ocp_nlp_dict[acados_struct]
+        setattr(acados_ocp, acados_struct, acados_attribute)
+
+    return acados_ocp
+
+
+def ocp_generate_external_functions(acados_ocp, model):
+
+    model = make_model_consistent(model)
+
+    if acados_ocp.solver_options.hessian_approx == 'EXACT':
+        opts = dict(generate_hess=1)
+    else:
+        opts = dict(generate_hess=0)
+    code_export_dir = acados_ocp.code_export_directory
+    opts['code_export_directory'] = code_export_dir
+
+    if acados_ocp.model.dyn_ext_fun_type != 'casadi':
+        raise Exception("ocp_generate_external_functions: dyn_ext_fun_type only supports 'casadi' for now.\
+            Extending the Python interface with generic function support is welcome.")
+
+    if acados_ocp.solver_options.integrator_type == 'ERK':
+        # explicit model -- generate C code
+        generate_c_code_explicit_ode(model, opts)
+    elif acados_ocp.solver_options.integrator_type == 'IRK':
+        # implicit model -- generate C code
+        generate_c_code_implicit_ode(model, opts)
+    elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK':
+        generate_c_code_implicit_ode(model, opts)
+    elif acados_ocp.solver_options.integrator_type == 'GNSF':
+        generate_c_code_gnsf(model, opts)
+    elif acados_ocp.solver_options.integrator_type == 'DISCRETE':
+        generate_c_code_discrete_dynamics(model, opts)
+    else:
+        raise Exception("ocp_generate_external_functions: unknown integrator type.")
+
+    if acados_ocp.dims.nphi > 0 or acados_ocp.dims.nh > 0:
+        generate_c_code_constraint(model, model.name, False, opts)
+
+    if acados_ocp.dims.nphi_e > 0 or acados_ocp.dims.nh_e > 0:
+        generate_c_code_constraint(model, model.name, True, opts)
+
+    # dummy matrices
+    if not acados_ocp.cost.cost_type_0 == 'LINEAR_LS':
+        acados_ocp.cost.Vx_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nx))
+        acados_ocp.cost.Vu_0 = np.zeros((acados_ocp.dims.ny_0, acados_ocp.dims.nu))
+    if not acados_ocp.cost.cost_type == 'LINEAR_LS':
+        acados_ocp.cost.Vx = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nx))
+        acados_ocp.cost.Vu = np.zeros((acados_ocp.dims.ny, acados_ocp.dims.nu))
+    if not acados_ocp.cost.cost_type_e == 'LINEAR_LS':
+        acados_ocp.cost.Vx_e = np.zeros((acados_ocp.dims.ny_e, acados_ocp.dims.nx))
+
+    if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
+        generate_c_code_nls_cost(model, model.name, 'initial', opts)
+    elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
+        generate_c_code_external_cost(model, 'initial', opts)
+
+    if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
+        generate_c_code_nls_cost(model, model.name, 'path', opts)
+    elif acados_ocp.cost.cost_type == 'EXTERNAL':
+        generate_c_code_external_cost(model, 'path', opts)
+
+    if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
+        generate_c_code_nls_cost(model, model.name, 'terminal', opts)
+    elif acados_ocp.cost.cost_type_e == 'EXTERNAL':
+        generate_c_code_external_cost(model, 'terminal', opts)
+
+
+def ocp_render_templates(acados_ocp, json_file):
+
+    name = acados_ocp.model.name
+
+    # setting up loader and environment
+    json_path = '{cwd}/{json_file}'.format(
+        cwd=os.getcwd(),
+        json_file=json_file)
+
+    if not os.path.exists(json_path):
+        raise Exception('{} not found!'.format(json_path))
+
+    code_export_dir = acados_ocp.code_export_directory
+    template_dir = code_export_dir
+
+    ## Render templates
+    in_file = 'main.in.c'
+    out_file = f'main_{name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'acados_solver.in.c'
+    out_file = f'acados_solver_{name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'acados_solver.in.h'
+    out_file = f'acados_solver_{name}.h'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'Makefile.in'
+    out_file = 'Makefile'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'acados_solver_sfun.in.c'
+    out_file = f'acados_solver_sfunction_{name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'make_sfun.in.m'
+    out_file = f'make_sfun_{name}.m'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    # sim
+    in_file = 'acados_sim_solver.in.c'
+    out_file = f'acados_sim_solver_{name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'acados_sim_solver.in.h'
+    out_file = f'acados_sim_solver_{name}.h'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'main_sim.in.c'
+    out_file = f'main_sim_{name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    ## folder model
+    template_dir = f'{code_export_dir}/{name}_model/'
+
+    in_file = 'model.in.h'
+    out_file = f'{name}_model.h'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    # constraints on convex over nonlinear function
+    if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0:
+        # constraints on outer function
+        template_dir = f'{code_export_dir}/{name}_constraints/'
+        in_file = 'phi_constraint.in.h'
+        out_file = f'{name}_phi_constraint.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # terminal constraints on convex over nonlinear function
+    if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0:
+        # terminal constraints on outer function
+        template_dir = f'{code_export_dir}/{name}_constraints/'
+        in_file = 'phi_e_constraint.in.h'
+        out_file = f'{name}_phi_e_constraint.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # nonlinear constraints
+    if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0:
+        template_dir = f'{code_export_dir}/{name}_constraints/'
+        in_file = 'h_constraint.in.h'
+        out_file = f'{name}_h_constraint.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # terminal nonlinear constraints
+    if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0:
+        template_dir = f'{code_export_dir}/{name}_constraints/'
+        in_file = 'h_e_constraint.in.h'
+        out_file = f'{name}_h_e_constraint.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # initial stage Nonlinear LS cost function
+    if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'cost_y_0_fun.in.h'
+        out_file = f'{name}_cost_y_0_fun.h'
+        render_template(in_file, out_file, template_dir, json_path)
+    # external cost - terminal
+    elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'external_cost_0.in.h'
+        out_file = f'{name}_external_cost_0.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # path Nonlinear LS cost function
+    if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'cost_y_fun.in.h'
+        out_file = f'{name}_cost_y_fun.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # terminal Nonlinear LS cost function
+    if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'cost_y_e_fun.in.h'
+        out_file = f'{name}_cost_y_e_fun.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # external cost
+    if acados_ocp.cost.cost_type == 'EXTERNAL':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'external_cost.in.h'
+        out_file = f'{name}_external_cost.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+    # external cost - terminal
+    if acados_ocp.cost.cost_type_e == 'EXTERNAL':
+        template_dir = f'{code_export_dir}/{name}_cost/'
+        in_file = 'external_cost_e.in.h'
+        out_file = f'{name}_external_cost_e.h'
+        render_template(in_file, out_file, template_dir, json_path)
+
+
+def remove_x0_elimination(acados_ocp):
+    acados_ocp.constraints.idxbxe_0 = np.zeros((0,))
+    acados_ocp.dims.nbxe_0 = 0
+
+
+class AcadosOcpSolver:
+    """
+    Class to interact with the acados ocp solver C object.
+
+        :param acados_ocp: type AcadosOcp - description of the OCP for acados
+        :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json
+        :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs
+    """
+    if sys.platform=="win32":
+        from ctypes import wintypes
+        dlclose = ctypes.WinDLL('kernel32', use_last_error=True).FreeLibrary
+        dlclose.argtypes = [wintypes.HMODULE]
+    else:
+        dlclose = CDLL(None).dlclose
+        dlclose.argtypes = [c_void_p]
+
+    @classmethod
+    def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True):
+        model = acados_ocp.model
+
+        if simulink_opts is None:
+            json_path = os.path.dirname(os.path.realpath(__file__))
+            with open(json_path + '/simulink_default_opts.json', 'r') as f:
+                simulink_opts = json.load(f)
+
+        # make dims consistent
+        make_ocp_dims_consistent(acados_ocp)
+
+        # module dependent post processing
+        if acados_ocp.solver_options.integrator_type == 'GNSF':
+            set_up_imported_gnsf_model(acados_ocp)
+
+        if acados_ocp.solver_options.qp_solver == 'PARTIAL_CONDENSING_QPDUNES':
+            remove_x0_elimination(acados_ocp)
+
+        # set integrator time automatically
+        acados_ocp.solver_options.Tsim = acados_ocp.solver_options.time_steps[0]
+
+        # generate external functions
+        ocp_generate_external_functions(acados_ocp, model)
+
+        # dump to json
+        ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file)
+
+        code_export_dir = acados_ocp.code_export_directory
+        # render templates
+        ocp_render_templates(acados_ocp, json_file)
+
+        if build:
+          ## Compile solver
+          cwd=os.getcwd()
+          os.chdir(code_export_dir)
+          os.system('make clean_ocp_shared_lib')
+          os.system('make ocp_shared_lib')
+          os.chdir(cwd)
+
+    def __init__(self, model_name, N, code_export_dir):
+        self.model_name = model_name
+        self.N = N
+
+        self.solver_created = False
+        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so'
+
+        # get shared_lib
+        self.shared_lib = CDLL(self.shared_lib_name)
+
+        # create capsule
+        getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule").restype = c_void_p
+        self.capsule = getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule")()
+
+        # create solver
+        getattr(self.shared_lib, f"{self.model_name}_acados_create").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_create").restype = c_int
+        assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0
+        self.solver_created = True
+
+        # get pointers solver
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p
+        self.nlp_opts = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts")(self.capsule)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").restype = c_void_p
+        self.nlp_dims = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims")(self.capsule)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").restype = c_void_p
+        self.nlp_config = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config")(self.capsule)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p
+        self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p
+        self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule)
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p
+        self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule)
+
+        # treat parameters separately
+        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
+        getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int
+        self._set_param = getattr(self.shared_lib, f"{self.model_name}_acados_update_params")
+
+        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
+        self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
+
+        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
+        self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
+
+        self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+        self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+        self.shared_lib.ocp_nlp_out_set.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+        self.shared_lib.ocp_nlp_set.argtypes = \
+            [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+        self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
+        self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+
+
+    def solve(self):
+        """
+        Solve the ocp with current input.
+        """
+
+        getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int
+        status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule)
+        return status
+
+
+    def get_slice(self, start_stage_, end_stage_, field_):
+        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+            self.nlp_dims, self.nlp_out, start_stage_, field_)
+        out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
+        self.fill_in_slice(start_stage_, end_stage_, field_, out)
+        return out
+
+    def fill_in_slice(self, start_stage_, end_stage_, field_, arr):
+        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
+        mem_fields = ['sl', 'su']
+        field = field_
+        field = field.encode('utf-8')
+
+        if (field_ not in out_fields + mem_fields):
+            raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
+                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+
+        if not isinstance(start_stage_, int):
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+
+        if not isinstance(end_stage_, int):
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+
+        if start_stage_ >= end_stage_:
+            raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
+
+        if start_stage_ < 0 or end_stage_ > self.N + 1:
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
+
+        out_data = cast(arr.ctypes.data, POINTER(c_double))
+
+        if (field_ in out_fields):
+            self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
+                self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
+        elif field_ in mem_fields:
+            self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
+                self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
+
+    def get(self, stage_, field_):
+        return self.get_slice(stage_, stage_ + 1, field_)[0]
+
+
+    def print_statistics(self):
+        """
+        prints statistics of previous solver run as a table:
+            - iter: iteration number
+            - res_stat: stationarity residual
+            - res_eq: residual wrt equality constraints (dynamics)
+            - res_ineq: residual wrt inequality constraints (constraints)
+            - res_comp: residual wrt complementarity conditions
+            - qp_stat: status of QP solver
+            - qp_iter: number of QP iterations
+            - qp_res_stat: stationarity residual of the last QP solution
+            - qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
+            - qp_res_ineq: residual wrt inequality constraints (constraints)  of the last QP solution
+            - qp_res_comp: residual wrt complementarity conditions of the last QP solution
+        """
+        stat = self.get_stats("statistics")
+
+        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
+            print('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter')
+            if stat.shape[0]>7:
+                print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
+            for jj in range(stat.shape[1]):
+                print('{:d}\t{:e}\t{:e}\t{:e}\t{:e}\t{:d}\t{:d}'.format( \
+                     int(stat[0][jj]), stat[1][jj], stat[2][jj], \
+                     stat[3][jj], stat[4][jj], int(stat[5][jj]), int(stat[6][jj])))
+                if stat.shape[0]>7:
+                    print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
+                        stat[7][jj], stat[8][jj], stat[9][jj], stat[10][jj]))
+            print('\n')
+        elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+            print('\niter\tqp_stat\tqp_iter')
+            if stat.shape[0]>3:
+                print('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp')
+            for jj in range(stat.shape[1]):
+                print('{:d}\t{:d}\t{:d}'.format( int(stat[0][jj]), int(stat[1][jj]), int(stat[2][jj])))
+                if stat.shape[0]>3:
+                    print('\t{:e}\t{:e}\t{:e}\t{:e}'.format( \
+                         stat[3][jj], stat[4][jj], stat[5][jj], stat[6][jj]))
+            print('\n')
+
+        return
+
+
+    def store_iterate(self, filename='', overwrite=False):
+        """
+        Stores the current iterate of the ocp solver in a json file.
+
+            :param filename: if not set, use model_name + timestamp + '.json'
+            :param overwrite: if false and filename exists add timestamp to filename
+        """
+        if filename == '':
+            filename += self.model_name + '_' + 'iterate' + '.json'
+
+        if not overwrite:
+            # append timestamp
+            if os.path.isfile(filename):
+                filename = filename[:-5]
+                filename += datetime.utcnow().strftime('%Y-%m-%d-%H:%M:%S.%f') + '.json'
+
+        # get iterate:
+        solution = dict()
+
+        for i in range(self.N+1):
+            solution['x_'+str(i)] = self.get(i,'x')
+            solution['u_'+str(i)] = self.get(i,'u')
+            solution['z_'+str(i)] = self.get(i,'z')
+            solution['lam_'+str(i)] = self.get(i,'lam')
+            solution['t_'+str(i)] = self.get(i, 't')
+            solution['sl_'+str(i)] = self.get(i, 'sl')
+            solution['su_'+str(i)] = self.get(i, 'su')
+        for i in range(self.N):
+            solution['pi_'+str(i)] = self.get(i,'pi')
+
+        # save
+        with open(filename, 'w') as f:
+            json.dump(solution, f, default=np_array_to_list, indent=4, sort_keys=True)
+        print("stored current iterate in ", os.path.join(os.getcwd(), filename))
+
+
+    def load_iterate(self, filename):
+        """
+        Loads the iterate stored in json file with filename into the ocp solver.
+        """
+        if not os.path.isfile(filename):
+            raise Exception('load_iterate: failed, file does not exist: ' + os.path.join(os.getcwd(), filename))
+
+        with open(filename, 'r') as f:
+            solution = json.load(f)
+
+        for key in solution.keys():
+            (field, stage) = key.split('_')
+            self.set(int(stage), field, np.array(solution[key]))
+
+
+    def get_stats(self, field_):
+        """
+        Get the information of the last solver call.
+
+            :param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
+        """
+
+        fields = ['time_tot',  # total cpu time previous call
+                  'time_lin',  # cpu time for linearization
+                  'time_sim',  # cpu time for integrator
+                  'time_sim_ad',  # cpu time for integrator contribution of external function calls
+                  'time_sim_la',  # cpu time for integrator contribution of linear algebra
+                  'time_qp',   # cpu time qp solution
+                  'time_qp_solver_call',  # cpu time inside qp solver (without converting the QP)
+                  'time_qp_xcond',
+                  'time_glob',  # cpu time globalization
+                  'time_reg',  # cpu time regularization
+                  'sqp_iter',  # number of SQP iterations
+                  'qp_iter',  # vector of QP iterations for last SQP call
+                  'statistics',  # table with info about last iteration
+                  'stat_m',
+                  'stat_n',]
+
+        field = field_
+        field = field.encode('utf-8')
+        if (field_ not in fields):
+            raise Exception('AcadosOcpSolver.get_stats(): {} is not a valid argument.\
+                    \n Possible values are {}. Exiting.'.format(fields, fields))
+
+        if field_ in ['sqp_iter', 'stat_m', 'stat_n']:
+            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.int64)
+            out_data = cast(out.ctypes.data, POINTER(c_int64))
+
+        elif field_ == 'statistics':
+            sqp_iter = self.get_stats("sqp_iter")
+            stat_m = self.get_stats("stat_m")
+            stat_n = self.get_stats("stat_n")
+
+            min_size = min([stat_m, sqp_iter+1])
+
+            out = np.ascontiguousarray(
+                        np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64)
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        elif field_ == 'qp_iter':
+            full_stats = self.get_stats('statistics')
+            if self.acados_ocp.solver_options.nlp_solver_type == 'SQP':
+                out = full_stats[6, :]
+            elif self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+                out = full_stats[2, :]
+
+        else:
+            out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        if not field_ == 'qp_iter':
+            self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        return out
+
+
+    def get_cost(self):
+        """
+        Returns the cost value of the current solution.
+        """
+        # compute cost internally
+        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
+        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+        out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        # call getter
+        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+
+        field = "cost_value".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        return out[0]
+
+
+    def get_residuals(self):
+        """
+        Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
+        """
+        # compute residuals if RTI
+        if self.acados_ocp.solver_options.nlp_solver_type == 'SQP_RTI':
+            self.shared_lib.ocp_nlp_eval_residuals.argtypes = [c_void_p, c_void_p, c_void_p]
+            self.shared_lib.ocp_nlp_eval_residuals(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        out = np.ascontiguousarray(np.zeros((4, 1)), dtype=np.float64)
+        out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        # call getters
+        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+
+        field = "res_stat".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        out_data = cast(out[1].ctypes.data, POINTER(c_double))
+        field = "res_eq".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        out_data = cast(out[2].ctypes.data, POINTER(c_double))
+        field = "res_ineq".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        out_data = cast(out[3].ctypes.data, POINTER(c_double))
+        field = "res_comp".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+        return out.flatten()
+
+
+    # Note: this function should not be used anymore, better use cost_set, constraints_set
+
+    def set(self, stage_, field_, value_):
+
+        """
+        Set numerical data inside the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
+
+            .. note:: regarding lam, t: \n
+                    the inequalities are internally organized in the following order: \n
+                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+            .. note:: pi: multipliers for dynamics equality constraints \n
+                      lam: multipliers for inequalities \n
+                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+                      sl: slack variables of soft lower inequality constraints \n
+                      su: slack variables of soft upper inequality constraints \n
+        """
+        cost_fields = ['y_ref', 'yref']
+        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
+        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
+        mem_fields = ['sl', 'su']
+
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        value_ = value_.astype(float)
+
+        field = field_
+        field = field.encode('utf-8')
+
+        stage = c_int(stage_)
+
+        if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
+            raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+                \nPossible values are {}. Exiting.".format(field, \
+                constraints_fields + cost_fields + out_fields + ['p']))
+
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+
+        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+            self.nlp_dims, self.nlp_out, stage_, field)
+
+        if value_.shape[0] != dims:
+            msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+            msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
+            raise Exception(msg)
+
+        value_data_p = cast(value_.ctypes.data, c_void_p)
+        if field_ in constraints_fields:
+            self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
+                self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+        elif field_ in cost_fields:
+            self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
+                self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+        elif field_ in out_fields:
+            self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
+                self.nlp_dims, self.nlp_out, stage, field, value_data_p)
+        elif field_ in mem_fields:
+            self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                self.nlp_solver, stage, field, value_data_p)
+        return
+
+
+    def set_param(self, stage_, value_):
+        value_data = cast(value_.ctypes.data, POINTER(c_double))
+        self._set_param(self.capsule, stage_, value_data, value_.shape[0])
+
+    def cost_set(self, start_stage_, field_, value_, api='warn'):
+      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+
+    def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+        """
+        Set numerical data in the cost module of the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
+            :param value: of appropriate size
+        """
+        # cast value_ to avoid conversion issues
+        field = field_.encode('utf-8')
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
+
+        self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \
+            self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
+            cast(value_.ctypes.data, c_void_p), dim)
+
+
+    def constraints_set(self, start_stage_, field_, value_, api='warn'):
+      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+
+
+    def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+        """
+        Set numerical data in the constraint module of the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
+            :param value: of appropriate size
+        """
+
+        field = field_.encode('utf-8')
+        if len(value_.shape) > 2:
+          dim = value_.shape[1]*value_.shape[2]
+        else:
+          dim = value_.shape[1]
+
+        self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
+            self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
+            cast(value_.ctypes.data, c_void_p), dim)
+
+
+    def dynamics_get(self, stage_, field_):
+        """
+        Get numerical data from the dynamics module of the solver:
+
+            :param stage: integer corresponding to shooting node
+            :param field: string, e.g. 'A'
+        """
+
+        field = field_
+        field = field.encode('utf-8')
+        stage = c_int(stage_)
+
+        # get dims
+        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr.restype = c_int
+
+        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+        dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+        self.shared_lib.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, \
+            self.nlp_dims, self.nlp_out, stage_, field, dims_data)
+
+        # create output data
+        out = np.ascontiguousarray(np.zeros((np.prod(dims),)), dtype=np.float64)
+        out = out.reshape(dims[0], dims[1], order='F')
+
+        out_data = cast(out.ctypes.data, POINTER(c_double))
+        out_data_p = cast((out_data), c_void_p)
+
+        # call getter
+        self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+        self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
+            self.nlp_dims, self.nlp_solver, stage, field, out_data_p)
+
+        return out
+
+
+    def options_set(self, field_, value_):
+        """
+        Set options of the solver.
+
+            :param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
+            :param value: of type int, float
+        """
+        int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
+        double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
+        string_fields = ['globalization']
+
+        # check field availability and type
+        if field_ in int_fields:
+            if not isinstance(value_, int):
+                raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
+            else:
+                value_ctypes = c_int(value_)
+
+        elif field_ in double_fields:
+            if not isinstance(value_, float):
+                raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
+            else:
+                value_ctypes = c_double(value_)
+
+        elif field_ in string_fields:
+            if not isinstance(value_, str):
+                raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
+            else:
+                value_ctypes = value_.encode('utf-8')
+        else:
+            raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
+                '\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
+
+
+        if field_ == 'rti_phase':
+            if value_ < 0 or value_ > 2:
+                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    'take only values 0, 1, 2 for SQP-RTI-type solvers')
+            if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
+                raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
+                    'take only value 0 for SQP-type solvers')
+
+        # encode
+        field = field_
+        field = field.encode('utf-8')
+
+        # call C interface
+        if field_ in string_fields:
+            self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \
+                [c_void_p, c_void_p, c_char_p, c_char_p]
+            self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \
+                self.nlp_opts, field, value_ctypes)
+        else:
+            self.shared_lib.ocp_nlp_solver_opts_set.argtypes = \
+                [c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \
+                self.nlp_opts, field, byref(value_ctypes))
+        return
+
+
+    def __del__(self):
+        if self.solver_created:
+            getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p]
+            getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int
+            getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule)
+
+            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").argtypes = [c_void_p]
+            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").restype = c_int
+            getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule")(self.capsule)
+
+            try:
+                self.dlclose(self.shared_lib._handle)
+            except:
+                pass
diff --git a/pyextra/acados_template/acados_ocp_solver_fast.py b/pyextra/acados_template/acados_ocp_solver_fast.py
new file mode 100644
index 000000000..656d288f1
--- /dev/null
+++ b/pyextra/acados_template/acados_ocp_solver_fast.py
@@ -0,0 +1,402 @@
+import sys
+import os
+import json
+import numpy as np
+from datetime import datetime
+
+from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p
+
+from copy import deepcopy
+
+from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
+from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
+from .generate_c_code_gnsf import generate_c_code_gnsf
+from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics
+from .generate_c_code_constraint import generate_c_code_constraint
+from .generate_c_code_nls_cost import generate_c_code_nls_cost
+from .generate_c_code_external_cost import generate_c_code_external_cost
+from .acados_ocp import AcadosOcp
+from .acados_model import acados_model_strip_casadi_symbolics
+from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\
+     format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\
+     set_up_imported_gnsf_model, get_acados_path
+
+
+class AcadosOcpSolverFast:
+    dlclose = CDLL(None).dlclose
+    dlclose.argtypes = [c_void_p]
+
+    def __init__(self, model_name, N, code_export_dir):
+
+        self.solver_created = False
+        self.N = N
+        self.model_name = model_name
+
+        self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so'
+
+        # get shared_lib
+        self.shared_lib = CDLL(self.shared_lib_name)
+
+        # create capsule
+        getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p
+        self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")()
+
+        # create solver
+        getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int
+        assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0
+        self.solver_created = True
+
+        # get pointers solver
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p
+        self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p
+        self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p
+        self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p
+        self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p
+        self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p
+        self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule)
+
+
+    def solve(self):
+        """
+        Solve the ocp with current input.
+        """
+        model_name = self.model_name
+
+        getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int
+        status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule)
+        return status
+
+    def cost_set(self, start_stage_, field_, value_, api='warn'):
+      self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+      return
+
+    def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+        """
+        Set numerical data in the cost module of the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess'
+            :param value: of appropriate size
+        """
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64)
+        field = field_
+        field = field.encode('utf-8')
+        dim = np.product(value_.shape[1:])
+
+        start_stage = c_int(start_stage_)
+        end_stage = c_int(end_stage_)
+        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+        self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int
+
+        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+        dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+        self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config,
+            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
+
+        value_shape = value_.shape
+        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
+        if len(value_shape) == 2:
+            value_shape = (value_shape[0], value_shape[1], 0)
+
+        elif len(value_shape) == 3:
+            if api=='old':
+                pass
+            elif api=='warn':
+                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
+                    raise Exception("Ambiguity in API detected.\n"
+                                    "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n"
+                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
+                                    "  You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) +
+                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
+                                    "  Two options to fix this error: \n" +
+                                    "   * Add api='old' to cost_set to restore old incorrect behaviour\n" +
+                                    "   * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " +
+                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
+                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
+                # Get elements in column major order
+                value_ = np.ravel(value_, order='F')
+            elif api=='new':
+                # Get elements in column major order
+                value_ = np.ravel(value_, order='F')
+            else:
+                raise Exception("Unknown api: '{}'".format(api))
+
+        if value_shape != expected_shape:
+            raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension',
+                            ' for field "{}" with dimension {} (you have {})'.format(
+                               field_, expected_shape, value_shape))
+
+
+        value_data = cast(value_.ctypes.data, POINTER(c_double))
+        value_data_p = cast((value_data), c_void_p)
+
+        self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
+        self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config,
+            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
+        return
+
+    def constraints_set(self, start_stage_, field_, value_, api='warn'):
+      self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
+      return
+
+    def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
+        """
+        Set numerical data in the constraint module of the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi']
+            :param value: of appropriate size
+        """
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        value_ = value_.astype(float)
+
+        field = field_
+        field = field.encode('utf-8')
+        dim = np.product(value_.shape[1:])
+
+        start_stage = c_int(start_stage_)
+        end_stage = c_int(end_stage_)
+        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)]
+        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int
+
+        dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+        dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+        self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
+            self.nlp_dims, self.nlp_out, start_stage_, field, dims_data)
+
+        value_shape = value_.shape
+        expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims]))
+        if len(value_shape) == 2:
+            value_shape = (value_shape[0], value_shape[1], 0)
+        elif len(value_shape) == 3:
+            if api=='old':
+                pass
+            elif api=='warn':
+                if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')):
+                    raise Exception("Ambiguity in API detected.\n"
+                                    "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n"
+                                    "Are you seeing this error suddenly in previously running code? Read on.\n"
+                                    "  You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) +
+                                    "  acados_template now correctly passes on any matrices to acados in column major format.\n" +
+                                    "  Two options to fix this error: \n" +
+                                    "   * Add api='old' to constraints_set to restore old incorrect behaviour\n" +
+                                    "   * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " +
+                                    "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " +
+                                    "If there is no such manipulation, then you have probably been getting an incorrect solution before.")
+                # Get elements in column major order
+                value_ = np.ravel(value_, order='F')
+            elif api=='new':
+                # Get elements in column major order
+                value_ = np.ravel(value_, order='F')
+            else:
+                raise Exception("Unknown api: '{}'".format(api))
+        if value_shape != expected_shape:
+            raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
+                ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape))
+
+        value_data = cast(value_.ctypes.data, POINTER(c_double))
+        value_data_p = cast((value_data), c_void_p)
+
+        self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int]
+        self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
+            self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim)
+        return
+
+    # Note: this function should not be used anymore, better use cost_set, constraints_set
+    def set(self, stage_, field_, value_):
+        """
+        Set numerical data inside the solver.
+
+            :param stage: integer corresponding to shooting node
+            :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p']
+
+            .. note:: regarding lam, t: \n
+                    the inequalities are internally organized in the following order: \n
+                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+            .. note:: pi: multipliers for dynamics equality constraints \n
+                      lam: multipliers for inequalities \n
+                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+                      sl: slack variables of soft lower inequality constraints \n
+                      su: slack variables of soft upper inequality constraints \n
+        """
+        cost_fields = ['y_ref', 'yref']
+        constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
+        out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
+        mem_fields = ['sl', 'su']
+
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+        value_ = value_.astype(float)
+
+        model_name = self.model_name
+
+        field = field_
+        field = field.encode('utf-8')
+
+        stage = c_int(stage_)
+
+        # treat parameters separately
+        if field_ == 'p':
+            getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)]
+            getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int
+
+            value_data = cast(value_.ctypes.data, POINTER(c_double))
+
+            assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0
+        else:
+            if field_ not in constraints_fields + cost_fields + out_fields + mem_fields:
+                raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
+                    \nPossible values are {}. Exiting.".format(field, \
+                    constraints_fields + cost_fields + out_fields + ['p']))
+
+            self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+            self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+
+            dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+                self.nlp_dims, self.nlp_out, stage_, field)
+
+            if value_.shape[0] != dims:
+                msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_)
+                msg += 'with dimension {} (you have {})'.format(dims, value_.shape)
+                raise Exception(msg)
+
+            value_data = cast(value_.ctypes.data, POINTER(c_double))
+            value_data_p = cast((value_data), c_void_p)
+
+            if field_ in constraints_fields:
+                self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \
+                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+            elif field_ in cost_fields:
+                self.shared_lib.ocp_nlp_cost_model_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \
+                    self.nlp_dims, self.nlp_in, stage, field, value_data_p)
+            elif field_ in out_fields:
+                self.shared_lib.ocp_nlp_out_set.argtypes = \
+                    [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_out_set(self.nlp_config, \
+                    self.nlp_dims, self.nlp_out, stage, field, value_data_p)
+            elif field_ in mem_fields:
+                self.shared_lib.ocp_nlp_set.argtypes = \
+                    [c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+                self.shared_lib.ocp_nlp_set(self.nlp_config, \
+                    self.nlp_solver, stage, field, value_data_p)
+        return
+
+
+    def get_slice(self, start_stage_, end_stage_, field_):
+        """
+        Get the last solution of the solver:
+
+            :param start_stage: integer corresponding to shooting node that indicates start of slice
+            :param end_stage: integer corresponding to shooting node that indicates end of slice
+            :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',]
+
+            .. note:: regarding lam, t: \n
+                    the inequalities are internally organized in the following order: \n
+                    [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n
+                      lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+            .. note:: pi: multipliers for dynamics equality constraints \n
+                      lam: multipliers for inequalities \n
+                      t: slack variables corresponding to evaluation of all inequalities (at the solution) \n
+                      sl: slack variables of soft lower inequality constraints \n
+                      su: slack variables of soft upper inequality constraints \n
+        """
+        out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
+        mem_fields = ['sl', 'su']
+        field = field_
+        field = field.encode('utf-8')
+
+        if (field_ not in out_fields + mem_fields):
+            raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\
+                    \n Possible values are {}. Exiting.'.format(field_, out_fields))
+
+        if not isinstance(start_stage_, int):
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+
+        if not isinstance(end_stage_, int):
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.')
+
+        if start_stage_ >= end_stage_:
+            raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
+
+        if start_stage_ < 0 or end_stage_ > self.N + 1:
+            raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N))
+        self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \
+            [c_void_p, c_void_p, c_void_p, c_int, c_char_p]
+        self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int
+
+        dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
+            self.nlp_dims, self.nlp_out, start_stage_, field)
+
+        out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64)
+        out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        if (field_ in out_fields):
+            self.shared_lib.ocp_nlp_out_get_slice.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \
+                self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data)
+        elif field_ in mem_fields:
+            self.shared_lib.ocp_nlp_get_at_stage.argtypes = \
+                [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
+            self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \
+                self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
+
+        return out
+
+    def get_cost(self):
+        """
+        Returns the cost value of the current solution.
+        """
+        # compute cost internally
+        self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p]
+        self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
+
+        # create output array
+        out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64)
+        out_data = cast(out.ctypes.data, POINTER(c_double))
+
+        # call getter
+        self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p]
+
+        field = "cost_value".encode('utf-8')
+        self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data)
+
+        return out[0]
diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py
new file mode 100644
index 000000000..3ae198865
--- /dev/null
+++ b/pyextra/acados_template/acados_sim.py
@@ -0,0 +1,294 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import numpy as np
+import casadi as ca
+import os
+from .acados_model import AcadosModel
+from .utils import get_acados_path
+
+class AcadosSimDims:
+    """
+    Class containing the dimensions of the model to be simulated.
+    """
+    def __init__(self):
+        self.__nx = None
+        self.__nu = None
+        self.__nz = 0
+        self.__np = 0
+
+    @property
+    def nx(self):
+        """:math:`n_x` - number of states. Type: int > 0"""
+        return self.__nx
+
+    @property
+    def nz(self):
+        """:math:`n_z` - number of algebraic variables. Type: int >= 0"""
+        return self.__nz
+
+    @property
+    def nu(self):
+        """:math:`n_u` - number of inputs. Type: int >= 0"""
+        return self.__nu
+
+    @property
+    def np(self):
+        """:math:`n_p` - number of parameters. Type: int >= 0"""
+        return self.__np
+
+    @nx.setter
+    def nx(self, nx):
+        if type(nx) == int and nx > 0:
+            self.__nx = nx
+        else:
+            raise Exception('Invalid nx value, expected positive integer. Exiting.')
+
+    @nz.setter
+    def nz(self, nz):
+        if type(nz) == int and nz > -1:
+            self.__nz = nz
+        else:
+            raise Exception('Invalid nz value, expected nonnegative integer. Exiting.')
+
+    @nu.setter
+    def nu(self, nu):
+        if type(nu) == int and nu > -1:
+            self.__nu = nu
+        else:
+            raise Exception('Invalid nu value, expected nonnegative integer. Exiting.')
+
+    @np.setter
+    def np(self, np):
+        if type(np) == int and np > -1:
+            self.__np = np
+        else:
+            raise Exception('Invalid np value, expected nonnegative integer. Exiting.')
+
+    def set(self, attr, value):
+        setattr(self, attr, value)
+
+
+class AcadosSimOpts:
+    """
+    class containing the solver options
+    """
+    def __init__(self):
+        self.__integrator_type = 'ERK'
+        self.__tf = None
+        # ints
+        self.__sim_method_num_stages = 1
+        self.__sim_method_num_steps = 1
+        self.__sim_method_newton_iter = 3
+        # bools
+        self.__sens_forw = True
+        self.__sens_adj = False
+        self.__sens_algebraic = False
+        self.__sens_hess = False
+        self.__output_z = False
+        self.__sim_method_jac_reuse = 0
+
+    @property
+    def integrator_type(self):
+        """Integrator type. Default: 'ERK'."""
+        return self.__integrator_type
+
+    @property
+    def num_stages(self):
+        """Number of stages in the integrator. Default: 1"""
+        return self.__sim_method_num_stages
+
+    @property
+    def num_steps(self):
+        """Number of steps in the integrator. Default: 1"""
+        return self.__sim_method_num_steps
+
+    @property
+    def newton_iter(self):
+        """Number of Newton iterations in simulation method. Default: 3"""
+        return self.__sim_method_newton_iter
+
+    @property
+    def sens_forw(self):
+        """Boolean determining if forward sensitivities are computed. Default: True"""
+        return self.__sens_forw
+
+    @property
+    def sens_adj(self):
+        """Boolean determining if adjoint sensitivities are computed. Default: False"""
+        return self.__sens_adj
+
+    @property
+    def sens_algebraic(self):
+        """Boolean determining if sensitivities wrt algebraic variables are computed. Default: False"""
+        return self.__sens_algebraic
+
+    @property
+    def sens_hess(self):
+        """Boolean determining if hessians are computed. Default: False"""
+        return self.__sens_hess
+
+    @property
+    def output_z(self):
+        """Boolean determining if values for algebraic variables (corresponding to start of simulation interval) are computed. Default: False"""
+        return self.__output_z
+
+    @property
+    def sim_method_jac_reuse(self):
+        """Integer determining if jacobians are reused (0 or 1). Default: 0"""
+        return self.__sim_method_jac_reuse
+
+    @property
+    def T(self):
+        """Time horizon"""
+        return self.__Tsim
+
+    @integrator_type.setter
+    def integrator_type(self, integrator_type):
+        integrator_types = ('ERK', 'IRK', 'GNSF')
+        if integrator_type in integrator_types:
+            self.__integrator_type = integrator_type
+        else:
+            raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
+                    + ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
+
+    @T.setter
+    def T(self, T):
+        self.__Tsim = T
+
+    @num_stages.setter
+    def num_stages(self, num_stages):
+        if isinstance(num_stages, int):
+            self.__sim_method_num_stages = num_stages
+        else:
+            raise Exception('Invalid num_stages value. num_stages must be an integer.')
+
+    @num_steps.setter
+    def num_steps(self, num_steps):
+        if isinstance(num_steps, int):
+            self.__sim_method_num_steps = num_steps
+        else:
+            raise Exception('Invalid num_steps value. num_steps must be an integer.')
+
+    @newton_iter.setter
+    def newton_iter(self, newton_iter):
+        if isinstance(newton_iter, int):
+            self.__sim_method_newton_iter = newton_iter
+        else:
+            raise Exception('Invalid newton_iter value. newton_iter must be an integer.')
+
+    @sens_forw.setter
+    def sens_forw(self, sens_forw):
+        if sens_forw in (True, False):
+            self.__sens_forw = sens_forw
+        else:
+            raise Exception('Invalid sens_forw value. sens_forw must be a Boolean.')
+
+    @sens_adj.setter
+    def sens_adj(self, sens_adj):
+        if sens_adj in (True, False):
+            self.__sens_adj = sens_adj
+        else:
+            raise Exception('Invalid sens_adj value. sens_adj must be a Boolean.')
+
+    @sens_hess.setter
+    def sens_hess(self, sens_hess):
+        if sens_hess in (True, False):
+            self.__sens_hess = sens_hess
+        else:
+            raise Exception('Invalid sens_hess value. sens_hess must be a Boolean.')
+
+    @sens_algebraic.setter
+    def sens_algebraic(self, sens_algebraic):
+        if sens_algebraic in (True, False):
+            self.__sens_algebraic = sens_algebraic
+        else:
+            raise Exception('Invalid sens_algebraic value. sens_algebraic must be a Boolean.')
+
+    @output_z.setter
+    def output_z(self, output_z):
+        if output_z in (True, False):
+            self.__output_z = output_z
+        else:
+            raise Exception('Invalid output_z value. output_z must be a Boolean.')
+
+    @sim_method_jac_reuse.setter
+    def sim_method_jac_reuse(self, sim_method_jac_reuse):
+        if sim_method_jac_reuse in (0, 1):
+            self.__sim_method_jac_reuse = sim_method_jac_reuse
+        else:
+            raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.')
+
+class AcadosSim:
+    """
+    The class has the following properties that can be modified to formulate a specific simulation problem, see below:
+
+    :param acados_path: string with the path to acados. It is used to generate the include and lib paths.
+
+    - :py:attr:`dims` of type :py:class:`acados_template.acados_ocp.AcadosSimDims` - are automatically detected from model
+    - :py:attr:`model` of type :py:class:`acados_template.acados_model.AcadosModel`
+    - :py:attr:`solver_options` of type :py:class:`acados_template.acados_sim.AcadosSimOpts`
+
+    - :py:attr:`acados_include_path` (set automatically)
+    - :py:attr:`acados_lib_path` (set automatically)
+    """
+    def __init__(self, acados_path=''):
+        if acados_path == '':
+            acados_path = get_acados_path()
+        self.dims = AcadosSimDims()
+        """Dimension definitions, automatically detected from :py:attr:`model`. Type :py:class:`acados_template.acados_sim.AcadosSimDims`"""
+        self.model = AcadosModel()
+        """Model definitions, type :py:class:`acados_template.acados_model.AcadosModel`"""
+        self.solver_options = AcadosSimOpts()
+        """Solver Options, type :py:class:`acados_template.acados_sim.AcadosSimOpts`"""
+
+        self.acados_include_path = f'{acados_path}/include'
+        """Path to acados include directors (set automatically), type: `string`"""
+        self.acados_lib_path = f'{acados_path}/lib'
+        """Path to where acados library is located (set automatically), type: `string`"""
+
+        self.code_export_directory = 'c_generated_code'
+        """Path to where code will be exported. Default: `c_generated_code`."""
+
+    def set(self, attr, value):
+        # tokenize string
+        tokens = attr.split('_', 1)
+        if len(tokens) > 1:
+            setter_to_call = getattr(getattr(self, tokens[0]), 'set')
+        else:
+            setter_to_call = getattr(self, 'set')
+
+        setter_to_call(tokens[1], value)
+
+        return
diff --git a/pyextra/acados_template/acados_sim_layout.json b/pyextra/acados_template/acados_sim_layout.json
new file mode 100644
index 000000000..c8f4280b8
--- /dev/null
+++ b/pyextra/acados_template/acados_sim_layout.json
@@ -0,0 +1,44 @@
+{
+    "acados_include_path": [
+        "str"
+    ],
+     "model": {
+         "name" : [
+            "str"
+        ]
+    },
+    "acados_lib_path": [
+        "str"
+    ],
+    "dims": {
+        "np": [
+            "int"
+        ],
+        "nu": [
+            "int"
+        ],
+        "nx": [
+            "int" 
+        ],
+        "nz": [
+            "int"
+        ]
+    },
+    "solver_options": {
+        "integrator_type": [
+            "str"
+        ],
+        "Tsim": [
+            "float"
+        ],
+        "sim_method_num_stages": [
+            "int"
+        ],
+        "sim_method_num_steps": [
+            "int"
+        ],
+        "sim_method_newton_iter": [
+            "int"
+        ]
+    }
+}
diff --git a/pyextra/acados_template/acados_sim_solver.py b/pyextra/acados_template/acados_sim_solver.py
new file mode 100644
index 000000000..a5447907b
--- /dev/null
+++ b/pyextra/acados_template/acados_sim_solver.py
@@ -0,0 +1,407 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import sys, os, json
+
+import numpy as np
+
+from ctypes import *
+from copy import deepcopy
+
+from .generate_c_code_explicit_ode import generate_c_code_explicit_ode
+from .generate_c_code_implicit_ode import generate_c_code_implicit_ode
+from .generate_c_code_gnsf import generate_c_code_gnsf
+from .acados_sim import AcadosSim
+from .acados_ocp import AcadosOcp
+from .acados_model import acados_model_strip_casadi_symbolics
+from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
+     make_model_consistent, set_up_imported_gnsf_model
+
+
+def make_sim_dims_consistent(acados_sim):
+    dims = acados_sim.dims
+    model = acados_sim.model
+    # nx
+    if is_column(model.x):
+        dims.nx = model.x.shape[0]
+    else:
+        raise Exception("model.x should be column vector!")
+
+    # nu
+    if is_column(model.u):
+        dims.nu = model.u.shape[0]
+    elif model.u == None or model.u == []:
+        dims.nu = 0
+    else:
+        raise Exception("model.u should be column vector or None!")
+
+    # nz
+    if is_column(model.z):
+        dims.nz = model.z.shape[0]
+    elif model.z == None or model.z == []:
+        dims.nz = 0
+    else:
+        raise Exception("model.z should be column vector or None!")
+
+    # np
+    if is_column(model.p):
+        dims.np = model.p.shape[0]
+    elif model.p == None or model.p == []:
+        dims.np = 0
+    else:
+        raise Exception("model.p should be column vector or None!")
+
+
+def get_sim_layout():
+    current_module = sys.modules[__name__]
+    acados_path = os.path.dirname(current_module.__file__)
+    with open(acados_path + '/acados_sim_layout.json', 'r') as f:
+        sim_layout = json.load(f)
+    return sim_layout
+
+
+
+def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
+    # Load acados_sim structure description
+    sim_layout = get_sim_layout()
+
+    # Copy input sim object dictionary
+    sim_dict = dict(deepcopy(acados_sim).__dict__)
+
+    for key, v in sim_layout.items():
+        # skip non dict attributes
+        if not isinstance(v, dict): continue
+        # Copy sim object attributes dictionaries
+        sim_dict[key]=dict(getattr(acados_sim, key).__dict__)
+
+    sim_dict['model'] = acados_model_strip_casadi_symbolics(sim_dict['model'])
+    sim_json = format_class_dict(sim_dict)
+
+    with open(json_file, 'w') as f:
+        json.dump(sim_json, f, default=np_array_to_list, indent=4, sort_keys=True)
+
+
+def sim_render_templates(json_file, model_name, code_export_dir):
+    # setting up loader and environment
+    json_path = '{cwd}/{json_file}'.format(
+        cwd=os.getcwd(),
+        json_file=json_file)
+
+    if not os.path.exists(json_path):
+        raise Exception(f"{json_path} not found!")
+
+    template_dir = code_export_dir
+
+    ## Render templates
+    in_file = 'acados_sim_solver.in.c'
+    out_file = f'acados_sim_solver_{model_name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'acados_sim_solver.in.h'
+    out_file = f'acados_sim_solver_{model_name}.h'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'Makefile.in'
+    out_file = f'Makefile'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    in_file = 'main_sim.in.c'
+    out_file = f'main_sim_{model_name}.c'
+    render_template(in_file, out_file, template_dir, json_path)
+
+    ## folder model
+    template_dir = f'{code_export_dir}/{model_name}_model/'
+
+    in_file = 'model.in.h'
+    out_file = f'{model_name}_model.h'
+    render_template(in_file, out_file, template_dir, json_path)
+
+
+def sim_generate_casadi_functions(acados_sim):
+    model = acados_sim.model
+    model = make_model_consistent(model)
+
+    integrator_type = acados_sim.solver_options.integrator_type
+
+    opts = dict(generate_hess = acados_sim.solver_options.sens_hess,
+                code_export_directory = acados_sim.code_export_directory)
+    # generate external functions
+    if integrator_type == 'ERK':
+        generate_c_code_explicit_ode(model, opts)
+    elif integrator_type == 'IRK':
+        generate_c_code_implicit_ode(model, opts)
+    elif integrator_type == 'GNSF':
+        generate_c_code_gnsf(model, opts)
+
+class AcadosSimSolver:
+    """
+    Class to interact with the acados integrator C object.
+
+    :param acados_sim: type :py:class:`acados_template.acados_ocp.AcadosOcp` (takes values to generate an instance :py:class:`acados_template.acados_sim.AcadosSim`) or :py:class:`acados_template.acados_sim.AcadosSim`
+    :param json_file: Default: 'acados_sim.json'
+    :param build: Default: True
+    """
+    def __init__(self, acados_sim_, json_file='acados_sim.json', build=True):
+
+        self.solver_created = False
+
+        if isinstance(acados_sim_, AcadosOcp):
+            # set up acados_sim_
+            acados_sim = AcadosSim()
+            acados_sim.model = acados_sim_.model
+            acados_sim.dims.nx = acados_sim_.dims.nx
+            acados_sim.dims.nu = acados_sim_.dims.nu
+            acados_sim.dims.nz = acados_sim_.dims.nz
+            acados_sim.dims.np = acados_sim_.dims.np
+            acados_sim.solver_options.integrator_type = acados_sim_.solver_options.integrator_type
+            acados_sim.code_export_directory = acados_sim_.code_export_directory
+
+        elif isinstance(acados_sim_, AcadosSim):
+            acados_sim = acados_sim_
+
+        acados_sim.__problem_class = 'SIM'
+
+        model_name = acados_sim.model.name
+        make_sim_dims_consistent(acados_sim)
+
+        # reuse existing json and casadi functions, when creating integrator from ocp
+        if isinstance(acados_sim_, AcadosSim):
+            if acados_sim.solver_options.integrator_type == 'GNSF':
+                set_up_imported_gnsf_model(acados_sim)
+
+            sim_generate_casadi_functions(acados_sim)
+            sim_formulation_json_dump(acados_sim, json_file)
+
+        code_export_dir = acados_sim.code_export_directory
+        if build:
+            # render templates
+            sim_render_templates(json_file, model_name, code_export_dir)
+
+            ## Compile solver
+            cwd = os.getcwd()
+            os.chdir(code_export_dir)
+            os.system('make sim_shared_lib')
+            os.chdir(cwd)
+
+        self.sim_struct = acados_sim
+        model_name = self.sim_struct.model.name
+        self.model_name = model_name
+
+        # Ctypes
+        shared_lib = f'{code_export_dir}/libacados_sim_solver_{model_name}.so'
+        self.shared_lib = CDLL(shared_lib)
+
+
+        # create capsule
+        getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule").restype = c_void_p
+        self.capsule = getattr(self.shared_lib, f"{model_name}_acados_sim_solver_create_capsule")()
+
+        # create solver
+        getattr(self.shared_lib, f"{model_name}_acados_sim_create").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_sim_create").restype = c_int
+        assert getattr(self.shared_lib, f"{model_name}_acados_sim_create")(self.capsule)==0
+        self.solver_created = True
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts").restype = c_void_p
+        self.sim_opts = getattr(self.shared_lib, f"{model_name}_acados_get_sim_opts")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims").restype = c_void_p
+        self.sim_dims = getattr(self.shared_lib, f"{model_name}_acados_get_sim_dims")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_config").restype = c_void_p
+        self.sim_config = getattr(self.shared_lib, f"{model_name}_acados_get_sim_config")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_out").restype = c_void_p
+        self.sim_out = getattr(self.shared_lib, f"{model_name}_acados_get_sim_out")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_in").restype = c_void_p
+        self.sim_in = getattr(self.shared_lib, f"{model_name}_acados_get_sim_in")(self.capsule)
+
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver").restype = c_void_p
+        self.sim_solver = getattr(self.shared_lib, f"{model_name}_acados_get_sim_solver")(self.capsule)
+
+        nu = self.sim_struct.dims.nu
+        nx = self.sim_struct.dims.nx
+        nz = self.sim_struct.dims.nz
+        self.gettable = {
+            'x': nx,
+            'xn': nx,
+            'u': nu,
+            'z': nz,
+            'S_forw': nx*(nx+nu),
+            'Sx': nx*nx,
+            'Su': nx*nu,
+            'S_adj': nx+nu,
+            'S_hess': (nx+nu)*(nx+nu),
+            'S_algebraic': (nz)*(nx+nu),
+        }
+
+        self.settable = ['S_adj', 'T', 'x', 'u', 'xdot', 'z', 'p'] # S_forw
+
+
+    def solve(self):
+        """
+        Solve the simulation problem with current input.
+        """
+        getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").argtypes = [c_void_p]
+        getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve").restype = c_int
+
+        status = getattr(self.shared_lib, f"{self.model_name}_acados_sim_solve")(self.capsule)
+        return status
+
+
+    def get(self, field_):
+        """
+        Get the last solution of the solver.
+
+            :param str field: string in ['x', 'u', 'z', 'S_forw', 'Sx', 'Su', 'S_adj', 'S_hess', 'S_algebraic']
+        """
+        field = field_
+        field = field.encode('utf-8')
+
+        if field_ in self.gettable.keys():
+
+            # allocate array
+            dims = self.gettable[field_]
+            out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
+            out_data = cast(out.ctypes.data, POINTER(c_double))
+
+            self.shared_lib.sim_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.sim_out_get(self.sim_config, self.sim_dims, self.sim_out, field, out_data)
+
+            if field_ == 'S_forw':
+                nu = self.sim_struct.dims.nu
+                nx = self.sim_struct.dims.nx
+                out = out.reshape(nx, nx+nu, order='F')
+            elif field_ == 'Sx':
+                nx = self.sim_struct.dims.nx
+                out = out.reshape(nx, nx, order='F')
+            elif field_ == 'Su':
+                nx = self.sim_struct.dims.nx
+                nu = self.sim_struct.dims.nu
+                out = out.reshape(nx, nu, order='F')
+            elif field_ == 'S_hess':
+                nx = self.sim_struct.dims.nx
+                nu = self.sim_struct.dims.nu
+                out = out.reshape(nx+nu, nx+nu, order='F')
+            elif field_ == 'S_algebraic':
+                nx = self.sim_struct.dims.nx
+                nu = self.sim_struct.dims.nu
+                nz = self.sim_struct.dims.nz
+                out = out.reshape(nz, nx+nu, order='F')
+        else:
+            raise Exception(f'AcadosSimSolver.get(): Unknown field {field_},' \
+                f' available fields are {", ".join(self.gettable.keys())}')
+
+        return out
+
+
+    def set(self, field_, value_):
+        """
+        Set numerical data inside the solver.
+
+            :param field: string in ['p', 'S_adj', 'T', 'x', 'u', 'xdot', 'z']
+            :param value: the value with appropriate size.
+        """
+        # cast value_ to avoid conversion issues
+        if isinstance(value_, (float, int)):
+            value_ = np.array([value_])
+
+        value_ = value_.astype(float)
+        value_data = cast(value_.ctypes.data, POINTER(c_double))
+        value_data_p = cast((value_data), c_void_p)
+
+        field = field_
+        field = field.encode('utf-8')
+
+        # treat parameters separately
+        if field_ == 'p':
+            model_name = self.sim_struct.model.name
+            getattr(self.shared_lib, f"{model_name}_acados_sim_update_params").argtypes = [c_void_p, POINTER(c_double), c_int]
+            value_data = cast(value_.ctypes.data, POINTER(c_double))
+            getattr(self.shared_lib, f"{model_name}_acados_sim_update_params")(self.capsule, value_data, value_.shape[0])
+            return
+        else:
+            # dimension check
+            dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc)
+            dims_data = cast(dims.ctypes.data, POINTER(c_int))
+
+            self.shared_lib.sim_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_char_p, POINTER(c_int)]
+            self.shared_lib.sim_dims_get_from_attr(self.sim_config, self.sim_dims, field, dims_data)
+
+            value_ = np.ravel(value_, order='F')
+
+            value_shape = value_.shape
+            if len(value_shape) == 1:
+                value_shape = (value_shape[0], 0)
+
+            if value_shape != tuple(dims):
+                raise Exception('AcadosSimSolver.set(): mismatching dimension' \
+                    ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
+
+        # set
+        if field_ in ['xdot', 'z']:
+            self.shared_lib.sim_solver_set.argtypes = [c_void_p, c_char_p, c_void_p]
+            self.shared_lib.sim_solver_set(self.sim_solver, field, value_data_p)
+        elif field_ in self.settable:
+            self.shared_lib.sim_in_set.argtypes = [c_void_p, c_void_p, c_void_p, c_char_p, c_void_p]
+            self.shared_lib.sim_in_set(self.sim_config, self.sim_dims, self.sim_in, field, value_data_p)
+        else:
+            raise Exception(f'AcadosSimSolver.set(): Unknown field {field_},' \
+                f' available fields are {", ".join(self.settable)}')
+
+        return
+
+
+    def __del__(self):
+
+        if self.solver_created:
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").argtypes = [c_void_p]
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_free").restype = c_int
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_free")(self.capsule)
+
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").argtypes = [c_void_p]
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule").restype = c_int
+            getattr(self.shared_lib, f"{self.model_name}_acados_sim_solver_free_capsule")(self.capsule)
+
+            try:
+                self.dlclose(self.shared_lib._handle)
+            except:
+                pass
diff --git a/pyextra/acados_template/c_templates_tera/CPPLINT.cfg b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg
new file mode 100644
index 000000000..bbd1caf05
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/CPPLINT.cfg
@@ -0,0 +1 @@
+exclude_files=[main, acados_solver, acados_solver_sfun, Makefile, model].*\.?
diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in
new file mode 100644
index 000000000..330a5aba1
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/Makefile.in
@@ -0,0 +1,499 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+{%- if solver_options.qp_solver %}
+	{%- set qp_solver = solver_options.qp_solver %}
+{%- else %}
+	{%- set qp_solver = "FULL_CONDENSING_HPIPM" %}
+{%- endif %}
+
+{%- if solver_options.hessian_approx %}
+	{%- set hessian_approx = solver_options.hessian_approx %}
+{%- elif solver_options.sens_hess %}
+	{%- set hessian_approx = "EXACT" %}
+{%- else %}
+	{%- set hessian_approx = "GAUSS_NEWTON" %}
+{%- endif %}
+
+{%- if constraints.constr_type %}
+	{%- set constr_type = constraints.constr_type %}
+{%- else %}
+	{%- set constr_type = "NONE" %}
+{%- endif %}
+
+{%- if constraints.constr_type_e %}
+	{%- set constr_type_e = constraints.constr_type_e %}
+{%- else %}
+	{%- set constr_type_e = "NONE" %}
+{%- endif %}
+
+{%- if cost.cost_type %}
+	{%- set cost_type = cost.cost_type %}
+{%- else %}
+	{%- set cost_type = "NONE" %}
+{%- endif %}
+
+{%- if cost.cost_type_e %}
+	{%- set cost_type_e = cost.cost_type_e %}
+{%- else %}
+	{%- set cost_type_e = "NONE" %}
+{%- endif %}
+
+{%- if cost.cost_type_0 %}
+	{%- set cost_type_0 = cost.cost_type_0 %}
+{%- else %}
+	{%- set cost_type_0 = "NONE" %}
+{%- endif %}
+
+{%- if dims.nh %}
+	{%- set dims_nh = dims.nh %}
+{%- else %}
+	{%- set dims_nh = 0 %}
+{%- endif %}
+
+{%- if dims.nphi %}
+	{%- set dims_nphi = dims.nphi %}
+{%- else %}
+	{%- set dims_nphi = 0 %}
+{%- endif %}
+
+{%- if dims.nh_e %}
+	{%- set dims_nh_e = dims.nh_e %}
+{%- else %}
+	{%- set dims_nh_e = 0 %}
+{%- endif %}
+
+{%- if dims.nphi_e %}
+	{%- set dims_nphi_e = dims.nphi_e %}
+{%- else %}
+	{%- set dims_nphi_e = 0 %}
+{%- endif %}
+{%- if solver_options.model_external_shared_lib_dir %}
+	{%- set model_external_shared_lib_dir = solver_options.model_external_shared_lib_dir %}
+{%- endif %}
+{%- if solver_options.model_external_shared_lib_name %}
+	{%- set model_external_shared_lib_name = solver_options.model_external_shared_lib_name %}
+{%- endif %}
+
+{# control operator #}
+{%- if os and os == "pc" %}
+	{%- set control = "&" %}
+{%- else %}
+	{%- set control = ";" %}
+{%- endif %}
+
+{# acados linking libraries and flags #}
+{%- if acados_link_libs and os and os == "pc" %}
+	{%- set link_libs = acados_link_libs.qpoases ~ " " ~ acados_link_libs.hpmpc ~ " " ~ acados_link_libs.osqp -%}
+	{%- set openmp_flag = acados_link_libs.openmp %}
+{%- else %}
+	{%- set openmp_flag = " " %}
+	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+		{%- set link_libs = "-lqpOASES_e" %}
+	{%- else %}
+		{%- set link_libs = "" %}
+	{%- endif %}
+{%- endif %}
+
+{# acados flags #}
+ACADOS_FLAGS = -fPIC -std=c99 {{ openmp_flag }} #-fno-diagnostics-show-line-numbers -g
+{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+ACADOS_FLAGS += -DACADOS_WITH_QPOASES
+{%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_OSQP" %}
+ACADOS_FLAGS += -DACADOS_WITH_OSQP
+{%- endif %}
+{%- if qp_solver == "PARTIAL_CONDENSING_QPDUNES" %}
+ACADOS_FLAGS += -DACADOS_WITH_QPDUNES
+{%- endif %}
+# # Debugging
+# ACADOS_FLAGS += -g3
+
+MODEL_OBJ=
+{%- if  solver_options.integrator_type == "ERK" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_fun.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_vde_forw.o
+{%- if hessian_approx == "EXACT" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_expl_ode_hess.o
+{%- endif %}
+{%- elif solver_options.integrator_type == "IRK" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o
+{%- if hessian_approx == "EXACT" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
+{%- endif %}
+{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o
+{%- if hessian_approx == "EXACT" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.o
+{%- elif solver_options.integrator_type == "DISCRETE" %}
+{%- if model.dyn_ext_fun_type == "casadi" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.o
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.o
+{%- if hessian_approx == "EXACT" %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.o
+{%- endif %}
+{%- else %}
+MODEL_OBJ+= {{ model.name }}_model/{{ model.dyn_source_discrete }}
+{%- endif %}
+{%- endif %}
+
+
+OCP_OBJ=
+{%- if constr_type == "BGP" and dims_nphi > 0 %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_constraint.o
+{%- endif %}
+{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.o
+{%- endif %}
+
+{%- if constr_type == "BGH" and dims_nh > 0 %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.o
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun.o
+{%- if hessian_approx == "EXACT" %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.o
+{%- endif %}
+{%- endif %}
+
+{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.o
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.o
+{%- if hessian_approx == "EXACT" %}
+OCP_OBJ+= {{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.o
+{%- endif %}
+{%- endif %}
+
+{%- if cost_type_0 == "NONLINEAR_LS" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c
+{%- elif cost_type_0 == "EXTERNAL" %}
+{% if cost.cost_ext_fun_type_0 == "casadi" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c
+{% else %}
+OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_0 }}
+{% endif %}
+{%- endif %}
+{%- if cost_type == "NONLINEAR_LS" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_hess.c
+{%- elif cost_type == "EXTERNAL" %}
+{% if cost.cost_ext_fun_type == "casadi" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c
+{% elif cost.cost_source_ext_cost != cost.cost_source_ext_cost_0 %}
+OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost }}
+{% endif %}
+{%- endif %}
+{%- if cost_type_e == "NONLINEAR_LS" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c
+{%- elif cost_type_e == "EXTERNAL" %}
+{% if cost.cost_ext_fun_type_e == "casadi" %}
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c
+OCP_OBJ+= {{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c
+{% elif cost.cost_source_ext_cost_e != cost.cost_source_ext_cost_0 %}
+OCP_OBJ+= {{ model.name }}_cost/{{ cost.cost_source_ext_cost_e }}
+{% endif %}
+{%- endif %}
+OCP_OBJ+= acados_solver_{{ model.name }}.o
+
+
+SIM_OBJ=
+SIM_OBJ+= acados_sim_solver_{{ model.name }}.o
+
+EX_OBJ=
+EX_OBJ+= main_{{ model.name }}.o
+
+EX_SIM_OBJ=
+EX_SIM_OBJ+= main_sim_{{ model.name }}.o
+
+OBJ=
+OBJ+= $(MODEL_OBJ)
+{%- if solver_options.integrator_type != "DISCRETE" %}
+OBJ+= $(SIM_OBJ)
+{%- endif %}
+OBJ+= $(OCP_OBJ)
+
+EXTERNAL_DIR=
+EXTERNAL_LIB=
+
+{%- if model_external_shared_lib_dir and model_external_shared_lib_name %}
+EXTERNAL_DIR+= {{ model_external_shared_lib_dir  }}
+EXTERNAL_LIB+= {{ model_external_shared_lib_name }}
+{%- endif %}
+
+INCLUDE_PATH = {{ acados_include_path }}
+LIB_PATH = {{ acados_lib_path }}
+
+{%- if solver_options.integrator_type == "DISCRETE" %}
+all: clean casadi_fun example
+shared_lib: ocp_shared_lib
+{%- else %}
+all: clean casadi_fun example_sim example
+shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
+{%- endif %}
+
+CASADI_MODEL_SOURCE= 
+{%- if solver_options.integrator_type == "ERK" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c 
+{%- if hessian_approx == "EXACT" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c 
+{%- endif %}
+{%- elif solver_options.integrator_type == "IRK" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
+{%- if hessian_approx == "EXACT" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
+{%- endif %}
+{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
+# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c
+# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c
+{%- if hessian_approx == "EXACT" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_jac_y_uhat.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_get_matrices_fun.c
+{%- elif solver_options.integrator_type == "DISCRETE" and model.dyn_ext_fun_type == "casadi" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun.c
+CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac.c
+{%- if hessian_approx == "EXACT" %}
+CASADI_MODEL_SOURCE+= {{ model.name }}_dyn_disc_phi_fun_jac_hess.c
+{%- endif %}
+{%- endif %}
+{%- if constr_type == "BGP" and dims_nphi > 0 %}
+CASADI_CON_PHI_SOURCE=
+CASADI_CON_PHI_SOURCE+= {{ model.name }}_phi_constraint.c
+{%- endif %}
+{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
+CASADI_CON_PHI_E_SOURCE=
+CASADI_CON_PHI_E_SOURCE+= {{ model.name }}_phi_e_constraint.c
+{%- endif %}
+{%- if constr_type == "BGH" and dims_nh > 0 %}
+CASADI_CON_H_SOURCE=
+CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt.c
+CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun.c
+{%- if hessian_approx == "EXACT" %}
+CASADI_CON_H_SOURCE+= {{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c
+{%- endif %}
+{%- endif %}
+
+{%- if dims_nh_e > 0 %}
+CASADI_CON_H_E_SOURCE=
+CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt.c
+CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun.c
+{%- if hessian_approx == "EXACT" %}
+CASADI_CON_H_E_SOURCE+= {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c
+{%- endif %}
+{%- endif %}
+
+{%- if cost_type == "NONLINEAR_LS" %}
+CASADI_COST_Y_SOURCE=
+CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun.c
+CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_fun_jac_ut_xt.c
+CASADI_COST_Y_SOURCE+= {{ model.name }}_cost_y_hess.c
+{%- endif %}
+{%- if cost_type_e == "NONLINEAR_LS" %}
+CASADI_COST_Y_E_SOURCE=
+CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun.c
+CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_fun_jac_ut_xt.c
+CASADI_COST_Y_E_SOURCE+= {{ model.name }}_cost_y_e_hess.c
+{%- endif %}
+{%- if cost_type_0 == "NONLINEAR_LS" %}
+CASADI_COST_Y_0_SOURCE=
+CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun.c
+CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_fun_jac_ut_xt.c
+CASADI_COST_Y_0_SOURCE+= {{ model.name }}_cost_y_0_hess.c
+{%- endif %}
+
+casadi_fun:
+	{%- if model.dyn_ext_fun_type == "casadi" %}
+	( cd {{ model.name }}_model {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE))
+	{%- endif %}
+	{%- if constr_type == "BGP" and dims_nphi > 0 %}
+	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_SOURCE))
+	{%- endif %}
+	{%- if constr_type_e == "BGP" and dims_nphi_e > 0 %}
+	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_PHI_E_SOURCE))
+	{%- endif %}
+	{%- if constr_type == "BGH" and dims_nh > 0 %}
+	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_SOURCE))
+	{%- endif %}
+	{%- if constr_type_e == "BGH" and dims_nh_e > 0 %}
+	( cd {{ model.name }}_constraints {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_CON_H_E_SOURCE))
+	{%- endif %}
+	{%- if cost_type == "NONLINEAR_LS" %}
+	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_SOURCE))
+	{%- endif %}
+	{%- if cost_type_e == "NONLINEAR_LS" %}
+	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_E_SOURCE))
+	{%- endif %}
+	{%- if cost_type_0 == "NONLINEAR_LS" %}
+	( cd {{ model.name }}_cost {{ control }} gcc $(ACADOS_FLAGS) -c $(CASADI_COST_Y_0_SOURCE))
+	{%- endif %}
+
+main:
+	gcc $(ACADOS_FLAGS) -c main_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
+	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+	-I $(INCLUDE_PATH)/qpOASES_e/
+	{%- endif %}
+
+main_sim:
+	gcc $(ACADOS_FLAGS) -c main_sim_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/
+
+ocp_solver:
+	gcc $(ACADOS_FLAGS) -c acados_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
+	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+	-I $(INCLUDE_PATH)/qpOASES_e/
+	{%- endif %}
+
+sim_solver:
+	gcc $(ACADOS_FLAGS) -c acados_sim_solver_{{ model.name }}.c -I $(INCLUDE_PATH)/blasfeo/include/ -I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) -I $(INCLUDE_PATH)/acados/ \
+	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+	-I $(INCLUDE_PATH)/qpOASES_e/
+	{%- endif %}
+
+example: ocp_solver main
+	gcc $(ACADOS_FLAGS) -o main_{{ model.name }} $(EX_OBJ) $(OBJ) -L $(LIB_PATH) \
+	-lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-I $(INCLUDE_PATH)/acados/ \
+	{%- if qp_solver == "FULL_CONDENSING_QPOASES" %}
+	-I $(INCLUDE_PATH)/qpOASES_e/
+	{%- endif %}
+
+
+example_sim: sim_solver main_sim
+	gcc $(ACADOS_FLAGS) -o main_sim_{{ model.name }} $(EX_SIM_OBJ) $(MODEL_OBJ) $(SIM_OBJ) -L $(LIB_PATH) \
+	-lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/acados/ \
+
+{%- if solver_options.integrator_type != "DISCRETE" %}
+
+bundled_shared_lib: casadi_fun ocp_solver sim_solver
+	gcc $(ACADOS_FLAGS) -shared -o libacados_solver_{{ model.name }}.so $(OBJ) \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-L $(LIB_PATH) \
+	-lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+
+ocp_shared_lib: casadi_fun ocp_solver
+	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
+	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+	
+{%- else %}
+
+ocp_shared_lib: casadi_fun ocp_solver
+	gcc $(ACADOS_FLAGS) -shared -o libacados_ocp_solver_{{ model.name }}.so $(OCP_OBJ) $(MODEL_OBJ) \
+	-I $(INCLUDE_PATH)/blasfeo/include/ \
+	-I $(INCLUDE_PATH)/hpipm/include/ \
+	-I $(INCLUDE_PATH) \
+	-L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
+	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+	
+{%- endif %}
+
+sim_shared_lib: casadi_fun sim_solver
+	gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
+	-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
+	{{ link_libs }} \
+	-lm \
+
+{%- if os and os == "pc" %}
+
+clean:
+	del \Q *.o 2>nul
+	del \Q *.so 2>nul
+	del \Q main_{{ model.name }} 2>nul
+
+clean_ocp_shared_lib:
+	del \Q libacados_ocp_solver_{{ model.name }}.so 2>nul
+	del \Q acados_solver_{{ model.name }}.o 2>nul
+
+{%- else %}
+
+clean:
+	rm -f *.o
+	rm -f *.so
+	rm -f main_{{ model.name }}
+
+clean_ocp_shared_lib:
+	rm -f libacados_ocp_solver_{{ model.name }}.so
+	rm -f acados_solver_{{ model.name }}.o
+
+{%- endif %}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
new file mode 100644
index 000000000..7fa08d8f7
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
@@ -0,0 +1,383 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// acados
+#include "acados/utils/print.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_solver_{{ model.name }}.h"
+
+// mex
+#include "mex.h"
+
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+    long long *l_ptr;
+    int status = 0;
+
+    // create solver
+    nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
+
+    status = {{ model.name }}_acados_create(acados_ocp_capsule);
+
+    if (status)
+    {
+        mexPrintf("{{ model.name }}_acados_create() returned status %d.\n", status);
+    }
+    mexPrintf("{{ model.name }}_acados_create() -> success!\n");
+
+    // get pointers to nlp solver related objects
+    ocp_nlp_plan *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
+    ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
+    ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
+    void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
+
+    // mexPrintf("acados: got pointer to objectes!\n");
+
+    // field names of output struct
+    #define FIELDS_OCP 9
+    #define FIELDS_EXT_FUN 25
+    #define MAX_FIELDS 25
+    char *fieldnames[MAX_FIELDS];
+
+    for (int i = 0; i < MAX_FIELDS; i++)
+    {
+        fieldnames[i] = (char*) mxMalloc(50);
+    }
+
+    memcpy(fieldnames[0],"config",sizeof("config"));
+    memcpy(fieldnames[1],"dims",sizeof("dims"));
+    memcpy(fieldnames[2],"opts",sizeof("opts"));
+    memcpy(fieldnames[3],"in",sizeof("in"));
+    memcpy(fieldnames[4],"out",sizeof("out"));
+    memcpy(fieldnames[5],"solver",sizeof("solver"));
+    memcpy(fieldnames[6],"sens_out",sizeof("sens_out"));
+    memcpy(fieldnames[7],"plan",sizeof("plan"));
+    memcpy(fieldnames[8],"capsule",sizeof("capsule"));
+
+    // create output struct - C_ocp
+    plhs[0] = mxCreateStructMatrix(1, 1, 9, (const char **) fieldnames);
+
+    // MEX: config, dims, opts, in, out, solver, sens_out, plan
+    // plan
+    mxArray *plan_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(plan_mat);
+    l_ptr[0] = (long long) nlp_plan;
+    mxSetField(plhs[0], 0, "plan", plan_mat);
+
+    // config
+    mxArray *config_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(config_mat);
+    l_ptr[0] = (long long) nlp_config;
+    mxSetField(plhs[0], 0, "config", config_mat);
+
+    // dims
+    mxArray *dims_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(dims_mat);
+    l_ptr[0] = (long long) nlp_dims;
+    mxSetField(plhs[0], 0, "dims", dims_mat);
+
+    // opts
+    mxArray *opts_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(opts_mat);
+    l_ptr[0] = (long long) nlp_opts;
+    mxSetField(plhs[0], 0, "opts", opts_mat);
+
+    // in
+    mxArray *in_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(in_mat);
+    l_ptr[0] = (long long) nlp_in;
+    mxSetField(plhs[0], 0, "in", in_mat);
+
+    // out
+    mxArray *out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(out_mat);
+    l_ptr[0] = (long long) nlp_out;
+    mxSetField(plhs[0], 0, "out", out_mat);
+
+    // solver
+    mxArray *solver_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(solver_mat);
+    l_ptr[0] = (long long) nlp_solver;
+    mxSetField(plhs[0], 0, "solver", solver_mat);
+
+    // TODO: sens_out not actually implemented in templates..
+    // sens_out
+    mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(sens_out_mat);
+    l_ptr[0] = (long long) 1;
+    mxSetField(plhs[0], 0, "sens_out", sens_out_mat);
+
+    // capsule
+    mxArray *capsule_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(capsule_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule;
+    mxSetField(plhs[0], 0, "capsule", capsule_mat);
+
+    /* store external function pointers */
+    // dyn
+    memcpy(fieldnames[0],"expl_ode_fun",sizeof("expl_ode_fun"));
+    memcpy(fieldnames[1],"forw_vde",sizeof("forw_vde"));
+    memcpy(fieldnames[2],"hess_vde",sizeof("hess_vde"));
+    memcpy(fieldnames[3],"impl_dae_fun",sizeof("impl_dae_fun"));
+    memcpy(fieldnames[4],"impl_dae_fun_jac_x_xdot_z",sizeof("impl_dae_fun_jac_x_xdot_z"));
+    memcpy(fieldnames[5],"impl_dae_jac_x_xdot_u_z",sizeof("impl_dae_jac_x_xdot_u_z"));
+    memcpy(fieldnames[6],"impl_dae_hess",sizeof("impl_dae_hess"));
+
+    memcpy(fieldnames[7],"gnsf_phi_fun",sizeof("gnsf_phi_fun"));
+    memcpy(fieldnames[8],"gnsf_phi_fun_jac_y",sizeof("gnsf_phi_fun_jac_y"));
+    memcpy(fieldnames[9],"gnsf_phi_jac_y_uhat",sizeof("gnsf_phi_jac_y_uhat"));
+    memcpy(fieldnames[10],"gnsf_f_lo_jac_x1_x1dot_u_z",sizeof("gnsf_f_lo_jac_x1_x1dot_u_z"));
+    memcpy(fieldnames[11],"gnsf_get_matrices_fun",sizeof("gnsf_get_matrices_fun"));
+
+    memcpy(fieldnames[12],"disc_phi_fun",sizeof("disc_phi_fun"));
+    memcpy(fieldnames[13],"disc_phi_fun_jac",sizeof("disc_phi_fun_jac"));
+    memcpy(fieldnames[14],"disc_phi_fun_jac_hess",sizeof("disc_phi_fun_jac_hess"));
+    
+    // cost
+    memcpy(fieldnames[15],"cost_y_fun",sizeof("cost_y_fun"));
+    memcpy(fieldnames[16],"cost_y_fun_jac_ut_xt",sizeof("cost_y_fun_jac_ut_xt"));
+    memcpy(fieldnames[17],"cost_y_hess",sizeof("cost_y_hess"));
+    memcpy(fieldnames[18],"ext_cost_fun",sizeof("ext_cost_fun"));
+    memcpy(fieldnames[19],"ext_cost_fun_jac",sizeof("ext_cost_fun_jac"));
+    memcpy(fieldnames[20],"ext_cost_fun_jac_hess",sizeof("ext_cost_fun_jac_hess"));
+
+    // constraints
+    memcpy(fieldnames[21],"phi_constraint",sizeof("phi_constraint"));
+    memcpy(fieldnames[22],"nl_constr_h_fun_jac",sizeof("nl_constr_h_fun_jac"));
+    memcpy(fieldnames[23],"nl_constr_h_fun",sizeof("nl_constr_h_fun"));
+    memcpy(fieldnames[24],"nl_constr_h_fun_jac_hess",sizeof("nl_constr_h_fun_jac_hess"));
+
+
+    // create output struct - C_ocp_ext_fun
+    plhs[1] = mxCreateStructMatrix(1, 1, FIELDS_EXT_FUN, (const char **) fieldnames);
+
+
+    for (int i = 0; i < FIELDS_EXT_FUN; i++)
+    {
+        mxFree( fieldnames[i] );
+    }
+
+/* dynamics */
+    mxArray *expl_ode_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *forw_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *hess_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *impl_dae_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *impl_dae_fun_jac_x_xdot_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *impl_dae_jac_x_xdot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *impl_dae_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+
+    mxArray *gnsf_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *gnsf_phi_fun_jac_y_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *gnsf_phi_jac_y_uhat_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *gnsf_f_lo_jac_x1_x1dot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *gnsf_get_matrices_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+
+    mxArray *disc_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *disc_phi_fun_jac_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+    mxArray *disc_phi_fun_jac_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
+
+{% if solver_options.integrator_type == "ERK" %}
+    {# TODO: remove _casadi from these names.. #}
+    l_ptr = mxGetData(forw_vde_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->forw_vde_casadi;
+    l_ptr = mxGetData(expl_ode_fun_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->expl_ode_fun;
+{% if solver_options.hessian_approx == "EXACT" %}
+    l_ptr = mxGetData(hess_vde_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->hess_vde_casadi;
+{%- endif %}
+{% elif solver_options.integrator_type == "IRK" %}
+    l_ptr = mxGetData(impl_dae_fun_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun;
+    l_ptr = mxGetData(impl_dae_fun_jac_x_xdot_z_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun_jac_x_xdot_z;
+    l_ptr = mxGetData(impl_dae_jac_x_xdot_u_z_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_jac_x_xdot_u_z;
+{% if solver_options.hessian_approx == "EXACT" %}
+    l_ptr = mxGetData(impl_dae_hess_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
+{%- endif %}
+{% elif solver_options.integrator_type == "GNSF" %}
+    l_ptr = mxGetData(gnsf_phi_fun_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
+    l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
+    l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
+    l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
+    l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
+{% elif solver_options.integrator_type == "DISCRETE" %}
+    l_ptr = mxGetData(disc_phi_fun_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun;
+    l_ptr = mxGetData(disc_phi_fun_jac_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt;
+{% if solver_options.hessian_approx == "EXACT" %}
+    l_ptr = mxGetData(disc_phi_fun_jac_hess_mat);
+    l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess;
+{%- endif %}
+{%- endif %}
+    mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat);
+    mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat);
+    mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat);
+
+    mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat);
+    mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat);
+    mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat);
+    mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat);
+    mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat);
+
+    mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat);
+    mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat);
+    mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat);
+    mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat);
+
+    mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat);
+    mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat);
+    mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat);
+/* constaints */
+    mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(phi_constraint_mat);
+{%- if constraints.constr_type == "BGP" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint;
+{% endif %}
+{% if constraints.constr_type_e == "BGP" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint;
+{% endif %}
+    mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat);
+
+    mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(nl_constr_h_fun_jac_mat);
+{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
+    l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac;
+{% endif %}
+{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac;
+{%- endif %}
+    mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat);
+
+    mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(nl_constr_h_fun_mat);
+{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
+    l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun;
+{% endif %}
+{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun;
+{%- endif %}
+    mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat);
+
+    mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat);
+{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess;
+{% endif %}
+{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess;
+{%- endif %}
+    mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat);
+
+/* cost */
+    mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(cost_y_fun_mat);
+{% if cost.cost_type == "NONLINEAR_LS" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun;
+{% endif %}
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun;
+{%- endif %}
+    mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat);
+
+    mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat);
+{% if cost.cost_type == "NONLINEAR_LS" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt;
+{% endif %}
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt;
+{%- endif %}
+    mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat);
+
+    mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(cost_y_hess_mat);
+{% if cost.cost_type == "NONLINEAR_LS" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess;
+{% endif %}
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess;
+{%- endif %}
+    mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat);
+
+    mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(ext_cost_fun_mat);
+{% if cost.cost_type == "EXTERNAL" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun;
+{% endif -%}
+{% if cost.cost_type_e == "EXTERNAL" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun;
+{%- endif %}
+    mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat);
+
+    mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(ext_cost_fun_jac_mat);
+{% if cost.cost_type == "EXTERNAL" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac;
+{% endif -%}
+{% if cost.cost_type_e == "EXTERNAL" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac;
+{%- endif %}
+    mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat);
+
+    mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
+    l_ptr = mxGetData(ext_cost_fun_jac_hess_mat);
+{% if cost.cost_type == "EXTERNAL" %}
+    l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess;
+{% endif -%}
+{% if cost.cost_type_e == "EXTERNAL" %}
+    l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess;
+{%- endif %}
+    mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat);
+
+
+    return;
+}
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
new file mode 100644
index 000000000..843440297
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+// system
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+// acados
+#include "acados_solver_{{ model.name }}.h"
+
+// mex
+#include "mex.h"
+
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+    int status = 0;
+    long long *ptr;
+
+    // mexPrintf("\nin mex_acados_free\n");
+    const mxArray *C_ocp = prhs[0];
+    // capsule
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
+    nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+
+    status = {{ model.name }}_acados_free(capsule);
+    if (status)
+    {
+        mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
+    }
+
+    status = {{ model.name }}_acados_free_capsule(capsule);
+    if (status)
+    {
+        mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status);
+    }
+
+    return;
+}
+
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
new file mode 100644
index 000000000..ee86a3116
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
@@ -0,0 +1,570 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+// acados
+#include "acados/utils/print.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_solver_{{ model.name }}.h"
+
+// mex
+#include "mex.h"
+#include "mex_macros.h"
+
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+
+    long long *ptr;
+    int acados_size;
+    mxArray *mex_field;
+    char fun_name[20] = "ocp_set";
+    char buffer [500]; // for error messages
+
+    /* RHS */
+    int min_nrhs = 6;
+
+    char *ext_fun_type = mxArrayToString( prhs[0] );
+    char *ext_fun_type_e = mxArrayToString( prhs[1] );
+
+    // C ocp
+    const mxArray *C_ocp = prhs[2];
+    // capsule
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
+    nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+    // plan
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
+    ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
+    // config
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
+    ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
+    // dims
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) );
+    ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0];
+    // opts
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) );
+    void *opts = (void *) ptr[0];
+    // in
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) );
+    ocp_nlp_in *in = (ocp_nlp_in *) ptr[0];
+    // out
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) );
+    ocp_nlp_out *out = (ocp_nlp_out *) ptr[0];
+    // solver
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) );
+    ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0];
+
+    const mxArray *C_ext_fun_pointers = prhs[3];
+    // field
+    char *field = mxArrayToString( prhs[4] );
+    // value
+    double *value = mxGetPr( prhs[5] );
+
+    // for checks
+    int matlab_size = (int) mxGetNumberOfElements( prhs[5] );
+    int nrow = (int) mxGetM( prhs[5] );
+    int ncol = (int) mxGetN( prhs[5] );
+
+    int N = dims->N;
+    int nu = dims->nu[0];
+    int nx = dims->nx[0];
+
+    // stage
+    int s0, se;
+    if (nrhs==min_nrhs)
+    {
+        s0 = 0;
+        se = N;
+    }
+    else if (nrhs==min_nrhs+1)
+    {
+        s0 = mxGetScalar( prhs[6] );
+        if (s0 > N)
+        {
+            sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0);
+            mexErrMsgTxt(buffer);            
+        }
+        se = s0 + 1;
+    }
+    else
+    {
+        sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs);
+        mexErrMsgTxt(buffer);
+    }
+
+    /* Set value */
+    // constraints
+    if (!strcmp(field, "constr_x0"))
+    {
+        acados_size = nx;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value);
+        ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value);
+    }
+    else if (!strcmp(field, "constr_C"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
+            MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value);
+        }
+    }
+    else if (!strcmp(field, "constr_lbx"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value);
+        }
+    }
+    else if (!strcmp(field, "constr_ubx"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value);
+        }
+    }
+    else if (!strcmp(field, "constr_lbu"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbu");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "lbu", value);
+        }
+    }
+    else if (!strcmp(field, "constr_ubu"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubu");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "ubu", value);
+        }
+    }
+    else if (!strcmp(field, "constr_D"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
+            MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value);
+        }
+    }
+    else if (!strcmp(field, "constr_lg"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value);
+        }
+    }
+    else if (!strcmp(field, "constr_ug"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value);
+        }
+    }
+    else if (!strcmp(field, "constr_lh"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lh");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "lh", value);
+        }
+    }
+    else if (!strcmp(field, "constr_uh"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "uh");
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+
+            ocp_nlp_constraints_model_set(config, dims, in, ii, "uh", value);
+        }
+    }
+    // cost:
+    else if (!strcmp(field, "cost_y_ref"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
+            {
+                acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value);
+            }
+            else
+            {
+                MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
+            }
+        }
+    }
+    else if (!strcmp(field, "cost_y_ref_e"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value);
+    }
+    else if (!strcmp(field, "cost_Vu"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
+            {
+                int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
+                int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u");
+                acados_size = ny * nu;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value);
+            }
+            else
+            {
+                MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
+            }
+        }
+    }
+    else if (!strcmp(field, "cost_Vx"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
+            {
+                int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
+                int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x");
+                acados_size = ny * nx;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value);
+            }
+            else
+            {
+                MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
+            }
+        }
+    }
+    else if (!strcmp(field, "cost_W"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
+            {
+                int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref");
+                acados_size = ny * ny;
+                MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+                ocp_nlp_cost_model_set(config, dims, in, ii, "W", value);
+            }
+            else
+            {
+                MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
+            }
+        }
+    }
+    else if (!strcmp(field, "cost_Z"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "Z", value);
+        }
+    }
+    else if (!strcmp(field, "cost_Zl"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zl");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "Zl", value);
+        }
+    }
+    else if (!strcmp(field, "cost_Zu"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zu");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "Zu", value);
+        }
+    }
+    else if (!strcmp(field, "cost_z"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_z");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "z", value);
+        }
+    }
+    else if (!strcmp(field, "cost_zl"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zl");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "zl", value);
+        }
+    }
+    else if (!strcmp(field, "cost_zu"))
+    {
+        acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zu");
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=s0; ii<se; ii++)
+        {
+            ocp_nlp_cost_model_set(config, dims, in, ii, "zu", value);
+        }
+    }
+    // constraints TODO
+    // // NOTE(oj): how is it with Jbx, Jbu, idxb can they be changed?!
+    // else if (!strcmp(field, "constr_lbx"))
+    // {
+    //     // bounds at 0 are a special case.
+    //     if (s0==0)
+    //     {
+    //         sprintf(buffer, "%s cannot set %s for stage 0", fun_name, field);
+    //         mexErrMsgTxt(buffer);
+    //     }
+    // }
+    // initializations
+    else if (!strcmp(field, "init_x"))
+    {
+        if (nrhs!=min_nrhs)
+            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+        acados_size = (N+1) * nx;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=0; ii<=N; ii++)
+        {
+            ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
+        }
+    }
+    else if (!strcmp(field, "init_u"))
+    {
+        if (nrhs!=min_nrhs)
+            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+        acados_size = N*nu;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=0; ii<N; ii++)
+        {
+            ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
+        }
+    }
+    else if (!strcmp(field, "init_z"))
+    {
+        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_t type = sim_plan.sim_solver;
+        if (type == IRK)
+        {
+            int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
+            if (nrhs!=min_nrhs)
+                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+            acados_size = N*nz;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<N; ii++)
+            {
+                ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
+            }
+        }
+        else
+        {
+            MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
+        }
+    }
+    else if (!strcmp(field, "init_xdot"))
+    {
+        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_t type = sim_plan.sim_solver;
+        if (type == IRK)
+        {
+            int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
+            if (nrhs!=min_nrhs)
+                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+            acados_size = N*nx;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<N; ii++)
+            {
+                ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
+            }
+        }
+        else
+        {
+            MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
+        }
+    }
+    else if (!strcmp(field, "init_gnsf_phi"))
+    {
+        sim_solver_plan sim_plan = plan->sim_solver_plan[0];
+        sim_solver_t type = sim_plan.sim_solver;
+        if (type == GNSF)
+        {
+            int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi");
+
+            if (nrhs!=min_nrhs)
+                MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+            acados_size = N*nout;
+            MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+            for (int ii=0; ii<N; ii++)
+            {
+                ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
+            }
+        }
+        else
+        {
+            MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
+        }
+    }
+    else if (!strcmp(field, "init_pi"))
+    {
+        if (nrhs!=min_nrhs)
+            MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
+
+        acados_size = N*nx;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        for (int ii=0; ii<N; ii++)
+        {
+            ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
+        }
+    }
+    else if (!strcmp(field, "init_lam"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lam");
+            MEX_DIM_CHECK_VEC(fun_name, "lam", nrow*ncol, nlam);
+
+            ocp_nlp_out_set(config, dims, out, ii, "lam", value);
+        }
+    }
+    else if (!strcmp(field, "init_t"))
+    {
+        for (int ii=s0; ii<se; ii++)
+        {
+            int nt = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "t");
+            MEX_DIM_CHECK_VEC(fun_name, "t", nrow*ncol, nt);
+
+            ocp_nlp_out_set(config, dims, out, ii, "t", value);
+        }
+    }
+    else if (!strcmp(field, "p"))
+    {
+        if (nrhs==min_nrhs) // all stages
+        {
+            for (int ii=0; ii<=N; ii++)
+            {
+                {{ model.name }}_acados_update_params(capsule, ii, value, matlab_size);
+            }
+        }
+        else if (nrhs==min_nrhs+1) // one stage
+        {
+            int stage = mxGetScalar( prhs[6] );
+            {{ model.name }}_acados_update_params(capsule, stage, value, matlab_size);
+        }
+    }
+    else if (!strcmp(field, "nlp_solver_max_iter"))
+    {
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int nlp_solver_max_iter = (int) value[0];
+        ocp_nlp_solver_opts_set(config, opts, "max_iter", &nlp_solver_max_iter);
+    }
+    else if (!strcmp(field, "rti_phase"))
+    {
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int rti_phase = (int) value[0];
+        if (plan->nlp_solver == SQP && rti_phase != 0)
+        {
+            MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti")
+        }
+        ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase);
+    }
+    else if (!strcmp(field, "qp_warm_start"))
+    {
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int qp_warm_start = (int) value[0];
+        ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start);
+    }
+    else if (!strcmp(field, "warm_start_first_qp"))
+    {
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int warm_start_first_qp = (int) value[0];
+        ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp);
+    }
+    else if (!strcmp(field, "print_level"))
+    {
+        acados_size = 1;
+        MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
+        int print_level = (int) value[0];
+        ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level);
+    }
+    else
+    {
+        MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
+ constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\
+ constr_lbu, constr_ubu, cost_y_ref[_e],\
+ cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
+ cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
+ init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");
+    }
+
+    return;
+}
+
diff --git a/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
new file mode 100644
index 000000000..9fd4feab2
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
@@ -0,0 +1,59 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+// system
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+// acados
+#include "acados_solver_{{ model.name }}.h"
+
+// mex
+#include "mex.h"
+
+
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+    // C_ocp
+    long long *ptr;
+    const mxArray *C_ocp = prhs[0];
+
+    // capsule
+    ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
+    nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
+
+    // solve
+    {{ model.name }}_acados_solve(capsule);
+
+}
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
new file mode 100644
index 000000000..390506420
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
@@ -0,0 +1,502 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+{%- if solver_options.hessian_approx %}
+	{%- set hessian_approx = solver_options.hessian_approx %}
+{%- elif solver_options.sens_hess %}
+	{%- set hessian_approx = "EXACT" %}
+{%- else %}
+	{%- set hessian_approx = "GAUSS_NEWTON" %}
+{%- endif %}
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+
+// acados
+#include "acados_c/external_function_interface.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/print.h"
+
+
+// example specific
+#include "{{ model.name }}_model/{{ model.name }}_model.h"
+#include "acados_sim_solver_{{ model.name }}.h"
+
+
+// ** solver data **
+
+sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(sim_solver_capsule));
+    sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
+{
+    // initialize
+    int nx = {{ dims.nx }};
+    int nu = {{ dims.nu }};
+    int nz = {{ dims.nz }};
+    bool tmp_bool;
+
+    {#// double Tsim = {{ solver_options.tf / dims.N }};#}
+    double Tsim = {{ solver_options.Tsim }};
+
+    {% if solver_options.integrator_type == "IRK" %}
+    capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    // external functions (implicit model)
+    capsule->sim_impl_dae_fun->casadi_fun  = &{{ model.name }}_impl_dae_fun;
+    capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
+    capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
+    capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
+    capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
+    capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
+    external_function_param_casadi_create(capsule->sim_impl_dae_fun, {{ dims.np }});
+
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
+    capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
+    external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, {{ dims.np }});
+
+    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
+    capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
+    external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, {{ dims.np }});
+
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess;
+    capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work;
+    capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
+    capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
+    capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
+    capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
+    external_function_param_casadi_create(capsule->sim_impl_dae_hess, {{ dims.np }});
+{%- endif %}
+
+    {% elif solver_options.integrator_type == "ERK" %}
+    // explicit ode
+    capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw;
+    capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
+    capsule->sim_forw_vde_casadi->casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
+    capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
+    capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
+    external_function_param_casadi_create(capsule->sim_forw_vde_casadi, {{ dims.np }});
+
+    capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
+    capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
+    capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
+    external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, {{ dims.np }});
+
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    // external_function_param_casadi impl_dae_jac_x_xdot_u_z;
+    capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess;
+    capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work;
+    capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
+    capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
+    capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
+    capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
+    external_function_param_casadi_create(capsule->sim_expl_ode_hess, {{ dims.np }});
+{%- endif %}
+
+    {% elif solver_options.integrator_type == "GNSF" -%}
+    capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+    capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
+
+    capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
+    capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
+    capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
+    capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
+    capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
+    capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
+    external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, {{ dims.np }});
+
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
+    capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
+    external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, {{ dims.np }});
+
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
+    capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
+    external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, {{ dims.np }});
+
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
+    external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, {{ dims.np }});
+
+    capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
+    capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
+    capsule->sim_gnsf_get_matrices_fun->casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
+    capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
+    capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
+    capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
+    external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, {{ dims.np }});
+    {% endif %}
+
+    // sim plan & config
+    sim_solver_plan plan;
+    plan.sim_solver = {{ solver_options.integrator_type }};
+
+    // create correct config based on plan
+    sim_config * {{ model.name }}_sim_config = sim_config_create(plan);
+    capsule->acados_sim_config = {{ model.name }}_sim_config;
+
+    // sim dims
+    void *{{ model.name }}_sim_dims = sim_dims_create({{ model.name }}_sim_config);
+    capsule->acados_sim_dims = {{ model.name }}_sim_dims;
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx", &nx);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nu", &nu);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz", &nz);
+{% if solver_options.integrator_type == "GNSF" %}
+    int gnsf_nx1 = {{ dims.gnsf_nx1 }};
+    int gnsf_nz1 = {{ dims.gnsf_nz1 }};
+    int gnsf_nout = {{ dims.gnsf_nout }};
+    int gnsf_ny = {{ dims.gnsf_ny }};
+    int gnsf_nuhat = {{ dims.gnsf_nuhat }};
+
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx1", &gnsf_nx1);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz1", &gnsf_nz1);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nout", &gnsf_nout);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "ny", &gnsf_ny);
+    sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nuhat", &gnsf_nuhat);
+{% endif %}
+
+    // sim opts
+    sim_opts *{{ model.name }}_sim_opts = sim_opts_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
+    capsule->acados_sim_opts = {{ model.name }}_sim_opts;
+    int tmp_int = {{ solver_options.sim_method_newton_iter }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
+
+{% if problem_class == "SIM" %}
+    tmp_int = {{ solver_options.sim_method_num_stages }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
+    tmp_int = {{ solver_options.sim_method_num_steps }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
+
+    // options that are not available to AcadosOcpSolver
+    //  (in OCP they will be determined by other options, like exact_hessian)
+    tmp_bool = {{ solver_options.sens_forw }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_forw", &tmp_bool);
+    tmp_bool = {{ solver_options.sens_adj }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_adj", &tmp_bool);
+    tmp_bool = {{ solver_options.sens_algebraic }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_algebraic", &tmp_bool);
+    tmp_bool = {{ solver_options.sens_hess }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_hess", &tmp_bool);
+    tmp_bool = {{ solver_options.output_z }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "output_z", &tmp_bool);
+
+{% else %} {# num_stages and num_steps of first shooting interval are used #}
+    tmp_int = {{ solver_options.sim_method_num_stages[0] }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
+    tmp_int = {{ solver_options.sim_method_num_steps[0] }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
+    tmp_bool = {{ solver_options.sim_method_jac_reuse[0] }};
+    sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool);
+{% endif %}
+
+    // sim in / out
+    sim_in *{{ model.name }}_sim_in = sim_in_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
+    capsule->acados_sim_in = {{ model.name }}_sim_in;
+    sim_out *{{ model.name }}_sim_out = sim_out_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
+    capsule->acados_sim_out = {{ model.name }}_sim_out;
+
+    sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
+               {{ model.name }}_sim_in, "T", &Tsim);
+
+    // model functions
+{%- if solver_options.integrator_type == "IRK" %}
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "impl_ode_fun", capsule->sim_impl_dae_fun);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z);
+{%- if hessian_approx == "EXACT" %}
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                "impl_dae_hess", capsule->sim_impl_dae_hess);
+{%- endif %}
+
+{%- elif solver_options.integrator_type == "ERK" %}
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "expl_vde_for", capsule->sim_forw_vde_casadi);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
+{%- if hessian_approx == "EXACT" %}
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                "expl_ode_hess", capsule->sim_expl_ode_hess);
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "phi_fun", capsule->sim_gnsf_phi_fun);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+    {{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
+                 "gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
+{%- endif %}
+
+    // sim solver
+    sim_solver *{{ model.name }}_sim_solver = sim_solver_create({{ model.name }}_sim_config,
+                                               {{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
+    capsule->acados_sim_solver = {{ model.name }}_sim_solver;
+
+    /* initialize parameter values */
+    {% if dims.np > 0 %}
+    // initialize parameters to nominal value
+    double p[{{ dims.np }}];
+    {% for i in range(end=dims.np) %}
+    p[{{ i }}] = {{ parameter_values[i] }};
+    {%- endfor %}
+
+{%- if solver_options.integrator_type == "ERK" %}
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
+{%- endif %}
+{%- elif solver_options.integrator_type == "IRK" %}
+    capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
+    capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
+    capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+    capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
+    capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
+    capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
+    capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
+{% endif %}
+    {% endif %}{# if dims.np #}
+
+    /* initialize input */
+    // x
+    double x0[{{ dims.nx }}];
+    for (int ii = 0; ii < {{ dims.nx }}; ii++)
+        x0[ii] = 0.0;
+
+    sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
+               {{ model.name }}_sim_in, "x", x0);
+
+
+    // u
+    double u0[{{ dims.nu }}];
+    for (int ii = 0; ii < {{ dims.nu }}; ii++)
+        u0[ii] = 0.0;
+
+    sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
+               {{ model.name }}_sim_in, "u", u0);
+
+    // S_forw
+    double S_forw[{{ dims.nx * (dims.nx + dims.nu) }}];
+    for (int ii = 0; ii < {{ dims.nx * (dims.nx + dims.nu) }}; ii++)
+        S_forw[ii] = 0.0;
+    for (int ii = 0; ii < {{ dims.nx }}; ii++)
+        S_forw[ii + ii * {{ dims.nx }} ] = 1.0;
+
+
+    sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
+               {{ model.name }}_sim_in, "S_forw", S_forw);
+
+    int status = sim_precompute({{ model.name }}_sim_solver, {{ model.name }}_sim_in, {{ model.name }}_sim_out);
+
+    return status;
+}
+
+
+int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule)
+{
+    // integrate dynamics using acados sim_solver
+    int status = sim_solve(capsule->acados_sim_solver,
+                           capsule->acados_sim_in, capsule->acados_sim_out);
+    if (status != 0)
+        printf("error in {{ model.name }}_acados_sim_solve()! Exiting.\n");
+
+    return status;
+}
+
+
+int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
+{
+    // free memory
+    sim_solver_destroy(capsule->acados_sim_solver);
+    sim_in_destroy(capsule->acados_sim_in);
+    sim_out_destroy(capsule->acados_sim_out);
+    sim_opts_destroy(capsule->acados_sim_opts);
+    sim_dims_destroy(capsule->acados_sim_dims);
+    sim_config_destroy(capsule->acados_sim_config);
+
+    // free external function
+{%- if solver_options.integrator_type == "IRK" %}
+    external_function_param_casadi_free(capsule->sim_impl_dae_fun);
+    external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
+    external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
+{%- if hessian_approx == "EXACT" %}
+    external_function_param_casadi_free(capsule->sim_impl_dae_hess);
+{%- endif %}
+{%- elif solver_options.integrator_type == "ERK" %}
+    external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
+    external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
+{%- if hessian_approx == "EXACT" %}
+    external_function_param_casadi_free(capsule->sim_expl_ode_hess);
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+    external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
+    external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
+    external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
+    external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
+    external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
+{% endif %}
+
+    return 0;
+}
+
+
+int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
+{
+    int status = 0;
+    int casadi_np = {{ dims.np }};
+
+    if (casadi_np != np) {
+        printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+
+{%- if solver_options.integrator_type == "ERK" %}
+    capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
+    capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
+{%- endif %}
+{%- elif solver_options.integrator_type == "IRK" %}
+    capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
+    capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
+    capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
+{%- if hessian_approx == "EXACT" %}
+    capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
+{%- endif %}
+{%- elif solver_options.integrator_type == "GNSF" %}
+    capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
+    capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
+    capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
+    capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
+    capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
+{% endif %}
+
+    return status;
+}
+
+/* getters pointers to C objects*/
+sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_config;
+};
+
+sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_in;
+};
+
+sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_out;
+};
+
+void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_dims;
+};
+
+sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_opts;
+};
+
+sim_solver  * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule)
+{
+    return capsule->acados_sim_solver;
+};
+
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
new file mode 100644
index 000000000..de332e835
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
@@ -0,0 +1,98 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SIM_{{ model.name }}_H_
+#define ACADOS_SIM_{{ model.name }}_H_
+
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// ** capsule for solver data **
+typedef struct sim_solver_capsule
+{
+    // acados objects
+    sim_in *acados_sim_in;
+    sim_out *acados_sim_out;
+    sim_solver *acados_sim_solver;
+    sim_opts *acados_sim_opts;
+    sim_config *acados_sim_config;
+    void *acados_sim_dims;
+
+    /* external functions */
+    // ERK
+    external_function_param_casadi * sim_forw_vde_casadi;
+    external_function_param_casadi * sim_expl_ode_fun_casadi;
+    external_function_param_casadi * sim_expl_ode_hess;
+
+    // IRK
+    external_function_param_casadi * sim_impl_dae_fun;
+    external_function_param_casadi * sim_impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi * sim_impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi * sim_impl_dae_hess;
+
+    // GNSF
+    external_function_param_casadi * sim_gnsf_phi_fun;
+    external_function_param_casadi * sim_gnsf_phi_fun_jac_y;
+    external_function_param_casadi * sim_gnsf_phi_jac_y_uhat;
+    external_function_param_casadi * sim_gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi * sim_gnsf_get_matrices_fun;
+
+} sim_solver_capsule;
+
+
+int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule);
+int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule);
+int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule);
+int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np);
+
+sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule);
+sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule);
+sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule);
+void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule);
+sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule);
+sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule);
+
+
+sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void);
+int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_SIM_{{ model.name }}_H_
diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
new file mode 100644
index 000000000..234295fa2
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver_sfun.in.c
@@ -0,0 +1,233 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME   acados_sim_solver_sfunction_{{ model.name }}
+#define S_FUNCTION_LEVEL  2
+
+#define MDL_START
+
+// acados
+#include "acados/utils/print.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "{{ model.name }}_model/{{ model.name }}_model.h"
+#include "acados_sim_solver_{{ model.name }}.h"
+
+#include "simstruc.h"
+
+#define SAMPLINGTIME {{ solver_options.Tsim }}
+
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+    // specify the number of continuous and discrete states
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);
+
+    {# compute number of input ports #}
+    {%- set n_inputs = 1 %}  {# x0 #}
+    {%- if dims.nu > 0 %}  {# u0 -#}
+        {%- set n_inputs = n_inputs + 1 -%}
+    {%- endif %}
+    {%- if dims.np > 0 %}  {# parameters #}
+        {%- set n_inputs = n_inputs + 1 -%}
+    {%- endif %}
+
+    // specify the number of input ports
+    if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
+        return;
+
+    // specify the number of output ports
+    if ( !ssSetNumOutputPorts(S, 1) )
+        return;
+
+    // specify dimension information for the input ports
+    {%- set i_input = 0 %}
+    // x0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }});
+
+    {%- if dims.nu > 0 %}
+    {%- set i_input = i_input + 1 %}
+    // u0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }});
+    {%- endif %}
+
+    {%- if dims.np > 0 %}
+    {%- set i_input = i_input + 1 %}
+    // parameters
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }});
+    {%- endif %}
+
+    // specify dimension information for the output ports
+    ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext
+
+    // specify the direct feedthrough status
+    // should be set to 1 for all inputs used in mdlOutputs
+    {%- for i in range(end=n_inputs) %}
+    ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
+    {%- endfor %}
+
+    // one sample time
+    ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+    ssSetSampleTime(S, 0, SAMPLINGTIME);
+    ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+    sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
+    {{ model.name }}_acados_sim_create(capsule);
+
+    ssSetUserData(S, (void*)capsule);
+}
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+    sim_solver_capsule *capsule = ssGetUserData(S);
+
+    sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
+    void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
+    // sim_opts * {{ model.name }}_acados_get_sim_opts(capsule);
+    // sim_solver * {{ model.name }}_acados_get_sim_solver(capsule);
+
+    InputRealPtrsType in_sign;
+    {% set input_sizes = [dims.nx, dims.nu, dims.np] %}
+
+    // local buffer
+    {%- set buffer_size =  input_sizes | sort | last %}
+    real_t buffer[{{ buffer_size }}];
+
+
+    /* go through inputs */
+    {%- set i_input = 0 %}
+    // initial condition
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.nx }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    sim_in_set(acados_sim_config, acados_sim_dims,
+               acados_sim_in, "x", buffer);
+
+
+    // ssPrintf("\nin acados sim:\n");
+    // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]);
+    //     ssPrintf("\n");
+
+{% if dims.nu > 0 %}
+    // control input - u
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.nu }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    sim_in_set(acados_sim_config, acados_sim_dims,
+               acados_sim_in, "u", buffer);
+{%- endif %}
+
+
+{% if dims.np > 0 %}
+    // parameters
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.np }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    // update value of parameters
+    {{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }});
+{%- endif %}
+
+
+    /* call solver */
+    int acados_status = {{ model.name }}_acados_sim_solve(capsule);
+
+
+    /* set outputs */
+    real_t *out_x = ssGetOutputPortRealSignal(S, 0);
+
+    // get simulated state
+    sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out,
+                "xn", (void *) out_x);
+
+    // ssPrintf("\nacados sim solve: returned %d\n", acados_status);
+    // for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]);
+    //     ssPrintf("\n");
+
+}
+
+
+static void mdlTerminate(SimStruct *S)
+{
+    sim_solver_capsule *capsule = ssGetUserData(S);
+
+    {{ model.name }}_acados_sim_free(capsule);
+    {{ model.name }}_acados_sim_solver_free_capsule(capsule);
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
new file mode 100644
index 000000000..da73f6e29
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c
@@ -0,0 +1,2446 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "{{ model.name }}_model/{{ model.name }}_model.h"
+{% if constraints.constr_type == "BGP" and dims.nphi %}
+#include "{{ model.name }}_constraints/{{ model.name }}_phi_constraint.h"
+{% endif %}
+{% if constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
+#include "{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.h"
+{% endif %}
+{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
+#include "{{ model.name }}_constraints/{{ model.name }}_h_constraint.h"
+{% endif %}
+{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+#include "{{ model.name }}_constraints/{{ model.name }}_h_e_constraint.h"
+{% endif %}
+{%- if cost.cost_type == "NONLINEAR_LS" %}
+#include "{{ model.name }}_cost/{{ model.name }}_cost_y_fun.h"
+{%- elif cost.cost_type == "EXTERNAL" %}
+#include "{{ model.name }}_cost/{{ model.name }}_external_cost.h"
+{%- endif %}
+{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+#include "{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.h"
+{%- elif cost.cost_type_0 == "EXTERNAL" %}
+#include "{{ model.name }}_cost/{{ model.name }}_external_cost_0.h"
+{%- endif %}
+{%- if cost.cost_type_e == "NONLINEAR_LS" %}
+#include "{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.h"
+{%- elif cost.cost_type_e == "EXTERNAL" %}
+#include "{{ model.name }}_cost/{{ model.name }}_external_cost_e.h"
+{%- endif %}
+
+#include "acados_solver_{{ model.name }}.h"
+
+#define NX     {{ dims.nx }}
+#define NZ     {{ dims.nz }}
+#define NU     {{ dims.nu }}
+#define NP     {{ dims.np }}
+#define NBX    {{ dims.nbx }}
+#define NBX0   {{ dims.nbx_0 }}
+#define NBU    {{ dims.nbu }}
+#define NSBX   {{ dims.nsbx }}
+#define NSBU   {{ dims.nsbu }}
+#define NSH    {{ dims.nsh }}
+#define NSG    {{ dims.nsg }}
+#define NSPHI  {{ dims.nsphi }}
+#define NSHN   {{ dims.nsh_e }}
+#define NSGN   {{ dims.nsg_e }}
+#define NSPHIN {{ dims.nsphi_e }}
+#define NSBXN  {{ dims.nsbx_e }}
+#define NS     {{ dims.ns }}
+#define NSN    {{ dims.ns_e }}
+#define NG     {{ dims.ng }}
+#define NBXN   {{ dims.nbx_e }}
+#define NGN    {{ dims.ng_e }}
+#define NY0    {{ dims.ny_0 }}
+#define NY     {{ dims.ny }}
+#define NYN    {{ dims.ny_e }}
+#define N      {{ dims.N }}
+#define NH     {{ dims.nh }}
+#define NPHI   {{ dims.nphi }}
+#define NHN    {{ dims.nh_e }}
+#define NPHIN  {{ dims.nphi_e }}
+#define NR     {{ dims.nr }}
+
+
+// ** solver data **
+
+nlp_solver_capsule * {{ model.name }}_acados_create_capsule()
+{
+    void* capsule_mem = malloc(sizeof(nlp_solver_capsule));
+    nlp_solver_capsule *capsule = (nlp_solver_capsule *) capsule_mem;
+
+    return capsule;
+}
+
+
+int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule)
+{
+    free(capsule);
+    return 0;
+}
+
+
+int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
+{
+    int status = 0;
+
+    // number of expected runtime parameters
+    capsule->nlp_np = NP;
+
+    /************************************************
+    *  plan & config
+    ************************************************/
+    ocp_nlp_plan * nlp_solver_plan = ocp_nlp_plan_create(N);
+    capsule->nlp_solver_plan = nlp_solver_plan;
+
+    {%- if solver_options.nlp_solver_type == "SQP" %}
+    nlp_solver_plan->nlp_solver = SQP;
+    {% else %}
+    nlp_solver_plan->nlp_solver = SQP_RTI;
+    {%- endif %}
+
+    nlp_solver_plan->ocp_qp_solver_plan.qp_solver = {{ solver_options.qp_solver }};
+
+    nlp_solver_plan->nlp_cost[0] = {{ cost.cost_type_0 }};
+    for (int i = 1; i < N; i++)
+        nlp_solver_plan->nlp_cost[i] = {{ cost.cost_type }};
+
+    nlp_solver_plan->nlp_cost[N] = {{ cost.cost_type_e }};
+
+    for (int i = 0; i < N; i++)
+    {
+        {% if solver_options.integrator_type == "DISCRETE" %}
+        nlp_solver_plan->nlp_dynamics[i] = DISCRETE_MODEL;
+        // discrete dynamics does not need sim solver option, this field is ignored
+        nlp_solver_plan->sim_solver_plan[i].sim_solver = INVALID_SIM_SOLVER;
+        {% else %}
+        nlp_solver_plan->nlp_dynamics[i] = CONTINUOUS_MODEL;
+        nlp_solver_plan->sim_solver_plan[i].sim_solver = {{ solver_options.integrator_type }};
+        {%- endif %}
+    }
+
+    for (int i = 0; i < N; i++)
+    {
+        {% if constraints.constr_type == "BGP" %}
+        nlp_solver_plan->nlp_constraints[i] = BGP;
+        {%- else -%}
+        nlp_solver_plan->nlp_constraints[i] = BGH;
+        {%- endif %}
+    }
+
+    {%- if constraints.constr_type_e == "BGP" %}
+    nlp_solver_plan->nlp_constraints[N] = BGP;
+    {% else %}
+    nlp_solver_plan->nlp_constraints[N] = BGH;
+    {%- endif %}
+
+{%- if solver_options.hessian_approx == "EXACT" %}
+    {%- if solver_options.regularize_method == "NO_REGULARIZE" %}
+    nlp_solver_plan->regularization = NO_REGULARIZE;
+    {%- elif solver_options.regularize_method == "MIRROR" %}
+    nlp_solver_plan->regularization = MIRROR;
+    {%- elif solver_options.regularize_method == "PROJECT" %}
+    nlp_solver_plan->regularization = PROJECT;
+    {%- elif solver_options.regularize_method == "PROJECT_REDUC_HESS" %}
+    nlp_solver_plan->regularization = PROJECT_REDUC_HESS;
+    {%- elif solver_options.regularize_method == "CONVEXIFY" %}
+    nlp_solver_plan->regularization = CONVEXIFY;
+    {%- endif %}
+{%- endif %}
+    ocp_nlp_config * nlp_config = ocp_nlp_config_create(*nlp_solver_plan);
+    capsule->nlp_config = nlp_config;
+
+
+    /************************************************
+    *  dimensions
+    ************************************************/
+    #define NINTNP1MEMS 17
+    int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS );
+
+    int* nx    = intNp1mem + (N+1)*0;
+    int* nu    = intNp1mem + (N+1)*1;
+    int* nbx   = intNp1mem + (N+1)*2;
+    int* nbu   = intNp1mem + (N+1)*3;
+    int* nsbx  = intNp1mem + (N+1)*4;
+    int* nsbu  = intNp1mem + (N+1)*5;
+    int* nsg   = intNp1mem + (N+1)*6;
+    int* nsh   = intNp1mem + (N+1)*7;
+    int* nsphi = intNp1mem + (N+1)*8;
+    int* ns    = intNp1mem + (N+1)*9;
+    int* ng    = intNp1mem + (N+1)*10;
+    int* nh    = intNp1mem + (N+1)*11;
+    int* nphi  = intNp1mem + (N+1)*12;
+    int* nz    = intNp1mem + (N+1)*13;
+    int* ny    = intNp1mem + (N+1)*14;
+    int* nr    = intNp1mem + (N+1)*15;
+    int* nbxe  = intNp1mem + (N+1)*16;
+
+    for (int i = 0; i < N+1; i++)
+    {
+        // common
+        nx[i]     = NX;
+        nu[i]     = NU;
+        nz[i]     = NZ;
+        ns[i]     = NS;
+        // cost
+        ny[i]     = NY;
+        // constraints
+        nbx[i]    = NBX;
+        nbu[i]    = NBU;
+        nsbx[i]   = NSBX;
+        nsbu[i]   = NSBU;
+        nsg[i]    = NSG;
+        nsh[i]    = NSH;
+        nsphi[i]  = NSPHI;
+        ng[i]     = NG;
+        nh[i]     = NH;
+        nphi[i]   = NPHI;
+        nr[i]     = NR;
+        nbxe[i]   = 0;
+    }
+
+    // for initial state
+    nbx[0]  = NBX0;
+    nsbx[0] = 0;
+    ns[0] = NS - NSBX;
+    nbxe[0] = {{ dims.nbxe_0 }};
+    ny[0] = NY0;
+
+    // terminal - common
+    nu[N]   = 0;
+    nz[N]   = 0;
+    ns[N]   = NSN;
+    // cost
+    ny[N]   = NYN;
+    // constraint
+    nbx[N]   = NBXN;
+    nbu[N]   = 0;
+    ng[N]    = NGN;
+    nh[N]    = NHN;
+    nphi[N]  = NPHIN;
+    nr[N]    = {{ dims.nr_e }};
+
+    nsbx[N]  = NSBXN;
+    nsbu[N]  = 0;
+    nsg[N]   = NSGN;
+    nsh[N]   = NSHN;
+    nsphi[N] = NSPHIN;
+
+    /* create and set ocp_nlp_dims */
+    ocp_nlp_dims * nlp_dims = ocp_nlp_dims_create(nlp_config);
+    capsule->nlp_dims = nlp_dims;
+
+    ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nx", nx);
+    ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nu", nu);
+    ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "nz", nz);
+    ocp_nlp_dims_set_opt_vars(nlp_config, nlp_dims, "ns", ns);
+
+    for (int i = 0; i <= N; i++)
+    {
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbx", &nbx[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbu", &nbu[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbx", &nsbx[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsbu", &nsbu[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "ng", &ng[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsg", &nsg[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nbxe", &nbxe[i]);
+    }
+
+    {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
+    ocp_nlp_dims_set_cost(nlp_config, nlp_dims, 0, "ny", &ny[0]);
+    {%- endif %}
+
+    {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
+    for (int i = 1; i < N; i++)
+        ocp_nlp_dims_set_cost(nlp_config, nlp_dims, i, "ny", &ny[i]);
+    {%- endif %}
+
+    for (int i = 0; i < N; i++)
+    {
+        {%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nh", &nh[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsh", &nsh[i]);
+        {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nr", &nr[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nphi", &nphi[i]);
+        ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, i, "nsphi", &nsphi[i]);
+        {%- endif %}
+    }
+
+    {%- if constraints.constr_type_e == "BGH" %}
+    ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nh", &nh[N]);
+    ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsh", &nsh[N]);
+    {%- elif constraints.constr_type_e == "BGP" %}
+    ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nr", &nr[N]);
+    ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nphi", &nphi[N]);
+    ocp_nlp_dims_set_constraints(nlp_config, nlp_dims, N, "nsphi", &nsphi[N]);
+    {%- endif %}
+    {%- if cost.cost_type_e == "NONLINEAR_LS" or cost.cost_type_e == "LINEAR_LS" %}
+    ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]);
+    {%- endif %}
+
+    free(intNp1mem);
+
+{% if solver_options.integrator_type == "GNSF" -%}
+    // GNSF specific dimensions
+    int gnsf_nx1 = {{ dims.gnsf_nx1 }};
+    int gnsf_nz1 = {{ dims.gnsf_nz1 }};
+    int gnsf_nout = {{ dims.gnsf_nout }};
+    int gnsf_ny = {{ dims.gnsf_ny }};
+    int gnsf_nuhat = {{ dims.gnsf_nuhat }};
+
+    for (int i = 0; i < N; i++)
+    {
+        if (nlp_solver_plan->sim_solver_plan[i].sim_solver == GNSF)
+        {
+            ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nx1", &gnsf_nx1);
+            ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nz1", &gnsf_nz1);
+            ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nout", &gnsf_nout);
+            ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_ny", &gnsf_ny);
+            ocp_nlp_dims_set_dynamics(nlp_config, nlp_dims, i, "gnsf_nuhat", &gnsf_nuhat);
+        }
+    }
+{%- endif %}
+
+    /************************************************
+    *  external functions
+    ************************************************/
+    {%- if constraints.constr_type == "BGP" %}
+    capsule->phi_constraint = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++)
+    {
+        // nonlinear part of convex-composite constraint
+        capsule->phi_constraint[i].casadi_fun = &{{ model.name }}_phi_constraint;
+        capsule->phi_constraint[i].casadi_n_in = &{{ model.name }}_phi_constraint_n_in;
+        capsule->phi_constraint[i].casadi_n_out = &{{ model.name }}_phi_constraint_n_out;
+        capsule->phi_constraint[i].casadi_sparsity_in = &{{ model.name }}_phi_constraint_sparsity_in;
+        capsule->phi_constraint[i].casadi_sparsity_out = &{{ model.name }}_phi_constraint_sparsity_out;
+        capsule->phi_constraint[i].casadi_work = &{{ model.name }}_phi_constraint_work;
+
+        external_function_param_casadi_create(&capsule->phi_constraint[i], {{ dims.np }});
+    }
+    {%- endif %}
+
+    {%- if constraints.constr_type_e == "BGP" %}
+    // nonlinear part of convex-composite constraint
+    capsule->phi_e_constraint.casadi_fun = &{{ model.name }}_phi_e_constraint;
+    capsule->phi_e_constraint.casadi_n_in = &{{ model.name }}_phi_e_constraint_n_in;
+    capsule->phi_e_constraint.casadi_n_out = &{{ model.name }}_phi_e_constraint_n_out;
+    capsule->phi_e_constraint.casadi_sparsity_in = &{{ model.name }}_phi_e_constraint_sparsity_in;
+    capsule->phi_e_constraint.casadi_sparsity_out = &{{ model.name }}_phi_e_constraint_sparsity_out;
+    capsule->phi_e_constraint.casadi_work = &{{ model.name }}_phi_e_constraint_work;
+
+    external_function_param_casadi_create(&capsule->phi_e_constraint, {{ dims.np }});
+    {% endif %}
+
+    {%- if constraints.constr_type == "BGH" and dims.nh > 0  %}
+    capsule->nl_constr_h_fun_jac = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->nl_constr_h_fun_jac[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt;
+        capsule->nl_constr_h_fun_jac[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_in;
+        capsule->nl_constr_h_fun_jac[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_n_out;
+        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in;
+        capsule->nl_constr_h_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out;
+        capsule->nl_constr_h_fun_jac[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_work;
+        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac[i], {{ dims.np }});
+    }
+    capsule->nl_constr_h_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->nl_constr_h_fun[i].casadi_fun = &{{ model.name }}_constr_h_fun;
+        capsule->nl_constr_h_fun[i].casadi_n_in = &{{ model.name }}_constr_h_fun_n_in;
+        capsule->nl_constr_h_fun[i].casadi_n_out = &{{ model.name }}_constr_h_fun_n_out;
+        capsule->nl_constr_h_fun[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_sparsity_in;
+        capsule->nl_constr_h_fun[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_sparsity_out;
+        capsule->nl_constr_h_fun[i].casadi_work = &{{ model.name }}_constr_h_fun_work;
+        external_function_param_casadi_create(&capsule->nl_constr_h_fun[i], {{ dims.np }});
+    }
+    {% if solver_options.hessian_approx == "EXACT" %}
+    capsule->nl_constr_h_fun_jac_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_fun = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess;
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in;
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out;
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in;
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out;
+        capsule->nl_constr_h_fun_jac_hess[i].casadi_work = &{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work;
+
+        external_function_param_casadi_create(&capsule->nl_constr_h_fun_jac_hess[i], {{ dims.np }});
+    }
+    {% endif %}
+    {% endif %}
+
+    {%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    capsule->nl_constr_h_e_fun_jac.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt;
+    capsule->nl_constr_h_e_fun_jac.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in;
+    capsule->nl_constr_h_e_fun_jac.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out;
+    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in;
+    capsule->nl_constr_h_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out;
+    capsule->nl_constr_h_e_fun_jac.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_work;
+    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac, {{ dims.np }});
+
+    capsule->nl_constr_h_e_fun.casadi_fun = &{{ model.name }}_constr_h_e_fun;
+    capsule->nl_constr_h_e_fun.casadi_n_in = &{{ model.name }}_constr_h_e_fun_n_in;
+    capsule->nl_constr_h_e_fun.casadi_n_out = &{{ model.name }}_constr_h_e_fun_n_out;
+    capsule->nl_constr_h_e_fun.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_sparsity_in;
+    capsule->nl_constr_h_e_fun.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_sparsity_out;
+    capsule->nl_constr_h_e_fun.casadi_work = &{{ model.name }}_constr_h_e_fun_work;
+    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun, {{ dims.np }});
+
+    {% if solver_options.hessian_approx == "EXACT" %}
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_fun = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess;
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in;
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out;
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in;
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out;
+    capsule->nl_constr_h_e_fun_jac_hess.casadi_work = &{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work;
+    external_function_param_casadi_create(&capsule->nl_constr_h_e_fun_jac_hess, {{ dims.np }});
+    {% endif %}
+    {%- endif %}
+
+{% if solver_options.integrator_type == "ERK" %}
+    // explicit ode
+    capsule->forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->forw_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_vde_forw;
+        capsule->forw_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
+        capsule->forw_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
+        capsule->forw_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
+        capsule->forw_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
+        capsule->forw_vde_casadi[i].casadi_work = &{{ model.name }}_expl_vde_forw_work;
+        external_function_param_casadi_create(&capsule->forw_vde_casadi[i], {{ dims.np }});
+    }
+
+    capsule->expl_ode_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->expl_ode_fun[i].casadi_fun = &{{ model.name }}_expl_ode_fun;
+        capsule->expl_ode_fun[i].casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
+        capsule->expl_ode_fun[i].casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
+        capsule->expl_ode_fun[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
+        capsule->expl_ode_fun[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
+        capsule->expl_ode_fun[i].casadi_work = &{{ model.name }}_expl_ode_fun_work;
+        external_function_param_casadi_create(&capsule->expl_ode_fun[i], {{ dims.np }});
+    }
+
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    capsule->hess_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->hess_vde_casadi[i].casadi_fun = &{{ model.name }}_expl_ode_hess;
+        capsule->hess_vde_casadi[i].casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
+        capsule->hess_vde_casadi[i].casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
+        capsule->hess_vde_casadi[i].casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
+        capsule->hess_vde_casadi[i].casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
+        capsule->hess_vde_casadi[i].casadi_work = &{{ model.name }}_expl_ode_hess_work;
+        external_function_param_casadi_create(&capsule->hess_vde_casadi[i], {{ dims.np }});
+    }
+    {%- endif %}
+
+{% elif solver_options.integrator_type == "IRK" %}
+    // implicit dae
+    capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
+        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
+        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
+        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
+        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
+        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+    }
+
+    capsule->impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
+        capsule->impl_dae_fun_jac_x_xdot_z[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_z[i], {{ dims.np }});
+    }
+
+    capsule->impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
+        capsule->impl_dae_jac_x_xdot_u_z[i].casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_jac_x_xdot_u_z[i], {{ dims.np }});
+    }
+
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    capsule->impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_hess[i].casadi_fun = &{{ model.name }}_impl_dae_hess;
+        capsule->impl_dae_hess[i].casadi_work = &{{ model.name }}_impl_dae_hess_work;
+        capsule->impl_dae_hess[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
+        capsule->impl_dae_hess[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
+        capsule->impl_dae_hess[i].casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
+        capsule->impl_dae_hess[i].casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_hess[i], {{ dims.np }});
+    }
+    {%- endif %}
+{% elif solver_options.integrator_type == "LIFTED_IRK" %}
+    // external functions (implicit model)
+    capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun;
+        capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work;
+        capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
+        capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
+        capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
+        capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }});
+    }
+
+    capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);    for (int i = 0; i < N; i++) {
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u;
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_work;
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in;
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out;
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in;
+        capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out;
+        external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_u[i], {{ dims.np }});
+    }
+
+{% elif solver_options.integrator_type == "GNSF" %}
+    capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->gnsf_phi_fun[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun;
+        capsule->gnsf_phi_fun[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
+        capsule->gnsf_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
+        capsule->gnsf_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
+        capsule->gnsf_phi_fun[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
+        capsule->gnsf_phi_fun[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
+        external_function_param_casadi_create(&capsule->gnsf_phi_fun[i], {{ dims.np }});
+    }
+
+    capsule->gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->gnsf_phi_fun_jac_y[i].casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
+        capsule->gnsf_phi_fun_jac_y[i].casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
+        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
+        capsule->gnsf_phi_fun_jac_y[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
+        capsule->gnsf_phi_fun_jac_y[i].casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
+        capsule->gnsf_phi_fun_jac_y[i].casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
+        external_function_param_casadi_create(&capsule->gnsf_phi_fun_jac_y[i], {{ dims.np }});
+    }
+
+    capsule->gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
+        capsule->gnsf_phi_jac_y_uhat[i].casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
+        external_function_param_casadi_create(&capsule->gnsf_phi_jac_y_uhat[i], {{ dims.np }});
+    }
+
+    capsule->gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i].casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
+        external_function_param_casadi_create(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i], {{ dims.np }});
+    }
+
+    capsule->gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N; i++) {
+        capsule->gnsf_get_matrices_fun[i].casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
+        capsule->gnsf_get_matrices_fun[i].casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
+        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
+        capsule->gnsf_get_matrices_fun[i].casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
+        capsule->gnsf_get_matrices_fun[i].casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
+        capsule->gnsf_get_matrices_fun[i].casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
+        external_function_param_casadi_create(&capsule->gnsf_get_matrices_fun[i], {{ dims.np }});
+    }
+{% elif solver_options.integrator_type == "DISCRETE" %}
+    // discrete dynamics
+    capsule->discr_dyn_phi_fun = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N);
+    for (int i = 0; i < N; i++)
+    {
+        {%- if model.dyn_ext_fun_type == "casadi" %}
+        capsule->discr_dyn_phi_fun[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun;
+        capsule->discr_dyn_phi_fun[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_n_in;
+        capsule->discr_dyn_phi_fun[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_n_out;
+        capsule->discr_dyn_phi_fun[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_sparsity_in;
+        capsule->discr_dyn_phi_fun[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_sparsity_out;
+        capsule->discr_dyn_phi_fun[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_work;
+        {%- else %}
+        capsule->discr_dyn_phi_fun[i].fun = &{{ model.dyn_disc_fun }};
+        {%- endif %}
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun[i], {{ dims.np }});
+    }
+
+    capsule->discr_dyn_phi_fun_jac_ut_xt = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N);
+    for (int i = 0; i < N; i++)
+    {
+        {%- if model.dyn_ext_fun_type == "casadi" %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac;
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_n_in;
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_n_out;
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in;
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out;
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_work;
+        {%- else %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt[i].fun = &{{ model.dyn_disc_fun_jac }};
+        {%- endif %}
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt[i], {{ dims.np }});
+    }
+
+  {%- if solver_options.hessian_approx == "EXACT" %}
+    capsule->discr_dyn_phi_fun_jac_ut_xt_hess = (external_function_param_{{ model.dyn_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ model.dyn_ext_fun_type }})*N);
+    for (int i = 0; i < N; i++)
+    {
+        {%- if model.dyn_ext_fun_type == "casadi" %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_fun = &{{ model.name }}_dyn_disc_phi_fun_jac_hess;
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in;
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_n_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out;
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_in = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in;
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_sparsity_out = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out;
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].casadi_work = &{{ model.name }}_dyn_disc_phi_fun_jac_hess_work;
+        {%- else %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i].fun = &{{ model.dyn_disc_fun_jac_hess }};
+        {%- endif %}
+        external_function_param_{{ model.dyn_ext_fun_type }}_create(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i], {{ dims.np }});
+    }
+  {%- endif %}
+{%- endif %}
+
+
+{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+    // nonlinear least square function
+    capsule->cost_y_0_fun.casadi_fun = &{{ model.name }}_cost_y_0_fun;
+    capsule->cost_y_0_fun.casadi_n_in = &{{ model.name }}_cost_y_0_fun_n_in;
+    capsule->cost_y_0_fun.casadi_n_out = &{{ model.name }}_cost_y_0_fun_n_out;
+    capsule->cost_y_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_sparsity_in;
+    capsule->cost_y_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_sparsity_out;
+    capsule->cost_y_0_fun.casadi_work = &{{ model.name }}_cost_y_0_fun_work;
+    external_function_param_casadi_create(&capsule->cost_y_0_fun, {{ dims.np }});
+
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_0_fun_jac_ut_xt;
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in;
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out;
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in;
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out;
+    capsule->cost_y_0_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_0_fun_jac_ut_xt_work;
+    external_function_param_casadi_create(&capsule->cost_y_0_fun_jac_ut_xt, {{ dims.np }});
+
+    capsule->cost_y_0_hess.casadi_fun = &{{ model.name }}_cost_y_0_hess;
+    capsule->cost_y_0_hess.casadi_n_in = &{{ model.name }}_cost_y_0_hess_n_in;
+    capsule->cost_y_0_hess.casadi_n_out = &{{ model.name }}_cost_y_0_hess_n_out;
+    capsule->cost_y_0_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_0_hess_sparsity_in;
+    capsule->cost_y_0_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_0_hess_sparsity_out;
+    capsule->cost_y_0_hess.casadi_work = &{{ model.name }}_cost_y_0_hess_work;
+    external_function_param_casadi_create(&capsule->cost_y_0_hess, {{ dims.np }});
+
+{%- elif cost.cost_type_0 == "EXTERNAL" %}
+    // external cost
+    {% if cost.cost_ext_fun_type_0 == "casadi" %}
+    capsule->ext_cost_0_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun;
+    capsule->ext_cost_0_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_n_in;
+    capsule->ext_cost_0_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_n_out;
+    capsule->ext_cost_0_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_in;
+    capsule->ext_cost_0_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_sparsity_out;
+    capsule->ext_cost_0_fun.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_work;
+    {% else %}
+    capsule->ext_cost_0_fun.fun = &{{ cost.cost_function_ext_cost_0 }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun, {{ dims.np }});
+
+    // external cost
+    {% if cost.cost_ext_fun_type_0 == "casadi" %}
+    capsule->ext_cost_0_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac;
+    capsule->ext_cost_0_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_in;
+    capsule->ext_cost_0_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_n_out;
+    capsule->ext_cost_0_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in;
+    capsule->ext_cost_0_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out;
+    capsule->ext_cost_0_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_work;
+    {% else %}
+    capsule->ext_cost_0_fun_jac.fun = &{{ cost.cost_function_ext_cost_0 }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac, {{ dims.np }});
+
+    // external cost
+    {% if cost.cost_ext_fun_type_0 == "casadi" %}
+    capsule->ext_cost_0_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess;
+    capsule->ext_cost_0_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in;
+    capsule->ext_cost_0_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out;
+    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in;
+    capsule->ext_cost_0_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out;
+    capsule->ext_cost_0_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_0_fun_jac_hess_work;
+    {% else %}
+    capsule->ext_cost_0_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_0 }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_create(&capsule->ext_cost_0_fun_jac_hess, {{ dims.np }});
+{%- endif %}
+
+{%- if cost.cost_type == "NONLINEAR_LS" %}
+    // nonlinear least squares cost
+    capsule->cost_y_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        capsule->cost_y_fun[i].casadi_fun = &{{ model.name }}_cost_y_fun;
+        capsule->cost_y_fun[i].casadi_n_in = &{{ model.name }}_cost_y_fun_n_in;
+        capsule->cost_y_fun[i].casadi_n_out = &{{ model.name }}_cost_y_fun_n_out;
+        capsule->cost_y_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_sparsity_in;
+        capsule->cost_y_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_sparsity_out;
+        capsule->cost_y_fun[i].casadi_work = &{{ model.name }}_cost_y_fun_work;
+
+        external_function_param_casadi_create(&capsule->cost_y_fun[i], {{ dims.np }});
+    }
+
+    capsule->cost_y_fun_jac_ut_xt = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_fun = &{{ model.name }}_cost_y_fun_jac_ut_xt;
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_in;
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_n_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_n_out;
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_in = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in;
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_sparsity_out = &{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out;
+        capsule->cost_y_fun_jac_ut_xt[i].casadi_work = &{{ model.name }}_cost_y_fun_jac_ut_xt_work;
+
+        external_function_param_casadi_create(&capsule->cost_y_fun_jac_ut_xt[i], {{ dims.np }});
+    }
+
+    capsule->cost_y_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        capsule->cost_y_hess[i].casadi_fun = &{{ model.name }}_cost_y_hess;
+        capsule->cost_y_hess[i].casadi_n_in = &{{ model.name }}_cost_y_hess_n_in;
+        capsule->cost_y_hess[i].casadi_n_out = &{{ model.name }}_cost_y_hess_n_out;
+        capsule->cost_y_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_y_hess_sparsity_in;
+        capsule->cost_y_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_y_hess_sparsity_out;
+        capsule->cost_y_hess[i].casadi_work = &{{ model.name }}_cost_y_hess_work;
+
+        external_function_param_casadi_create(&capsule->cost_y_hess[i], {{ dims.np }});
+    }
+{%- elif cost.cost_type == "EXTERNAL" %}
+    // external cost
+    capsule->ext_cost_fun = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        {% if cost.cost_ext_fun_type == "casadi" %}
+        capsule->ext_cost_fun[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun;
+        capsule->ext_cost_fun[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_n_in;
+        capsule->ext_cost_fun[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_n_out;
+        capsule->ext_cost_fun[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_sparsity_in;
+        capsule->ext_cost_fun[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_sparsity_out;
+        capsule->ext_cost_fun[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_work;
+        {% else %}
+        capsule->ext_cost_fun[i].fun = &{{ cost.cost_function_ext_cost }};
+        {% endif %}
+        external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun[i], {{ dims.np }});
+    }
+
+    capsule->ext_cost_fun_jac = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        {% if cost.cost_ext_fun_type == "casadi" %}
+        capsule->ext_cost_fun_jac[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac;
+        capsule->ext_cost_fun_jac[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_n_in;
+        capsule->ext_cost_fun_jac[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_n_out;
+        capsule->ext_cost_fun_jac[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in;
+        capsule->ext_cost_fun_jac[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out;
+        capsule->ext_cost_fun_jac[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_work;
+        {% else %}
+        capsule->ext_cost_fun_jac[i].fun = &{{ cost.cost_function_ext_cost }};
+        {% endif %}
+        external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac[i], {{ dims.np }});
+    }
+
+    capsule->ext_cost_fun_jac_hess = (external_function_param_{{ cost.cost_ext_fun_type }} *) malloc(sizeof(external_function_param_{{ cost.cost_ext_fun_type }})*N);
+    for (int i = 0; i < N-1; i++)
+    {
+        {% if cost.cost_ext_fun_type == "casadi" %}
+        capsule->ext_cost_fun_jac_hess[i].casadi_fun = &{{ model.name }}_cost_ext_cost_fun_jac_hess;
+        capsule->ext_cost_fun_jac_hess[i].casadi_n_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_in;
+        capsule->ext_cost_fun_jac_hess[i].casadi_n_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_n_out;
+        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in;
+        capsule->ext_cost_fun_jac_hess[i].casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out;
+        capsule->ext_cost_fun_jac_hess[i].casadi_work = &{{ model.name }}_cost_ext_cost_fun_jac_hess_work;
+        {% else %}
+        capsule->ext_cost_fun_jac_hess[i].fun = &{{ cost.cost_function_ext_cost }};
+        {% endif %}
+        external_function_param_{{ cost.cost_ext_fun_type }}_create(&capsule->ext_cost_fun_jac_hess[i], {{ dims.np }});
+    }
+{%- endif %}
+
+{%- if cost.cost_type_e == "NONLINEAR_LS" %}
+    // nonlinear least square function
+    capsule->cost_y_e_fun.casadi_fun = &{{ model.name }}_cost_y_e_fun;
+    capsule->cost_y_e_fun.casadi_n_in = &{{ model.name }}_cost_y_e_fun_n_in;
+    capsule->cost_y_e_fun.casadi_n_out = &{{ model.name }}_cost_y_e_fun_n_out;
+    capsule->cost_y_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_sparsity_in;
+    capsule->cost_y_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_sparsity_out;
+    capsule->cost_y_e_fun.casadi_work = &{{ model.name }}_cost_y_e_fun_work;
+    external_function_param_casadi_create(&capsule->cost_y_e_fun, {{ dims.np }});
+
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_fun = &{{ model.name }}_cost_y_e_fun_jac_ut_xt;
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in;
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_n_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out;
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_in = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in;
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_sparsity_out = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out;
+    capsule->cost_y_e_fun_jac_ut_xt.casadi_work = &{{ model.name }}_cost_y_e_fun_jac_ut_xt_work;
+    external_function_param_casadi_create(&capsule->cost_y_e_fun_jac_ut_xt, {{ dims.np }});
+
+    capsule->cost_y_e_hess.casadi_fun = &{{ model.name }}_cost_y_e_hess;
+    capsule->cost_y_e_hess.casadi_n_in = &{{ model.name }}_cost_y_e_hess_n_in;
+    capsule->cost_y_e_hess.casadi_n_out = &{{ model.name }}_cost_y_e_hess_n_out;
+    capsule->cost_y_e_hess.casadi_sparsity_in = &{{ model.name }}_cost_y_e_hess_sparsity_in;
+    capsule->cost_y_e_hess.casadi_sparsity_out = &{{ model.name }}_cost_y_e_hess_sparsity_out;
+    capsule->cost_y_e_hess.casadi_work = &{{ model.name }}_cost_y_e_hess_work;
+    external_function_param_casadi_create(&capsule->cost_y_e_hess, {{ dims.np }});
+
+{%- elif cost.cost_type_e == "EXTERNAL" %}
+    // external cost
+    {% if cost.cost_ext_fun_type_e == "casadi" %}
+    capsule->ext_cost_e_fun.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun;
+    capsule->ext_cost_e_fun.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_n_in;
+    capsule->ext_cost_e_fun.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_n_out;
+    capsule->ext_cost_e_fun.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_in;
+    capsule->ext_cost_e_fun.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_sparsity_out;
+    capsule->ext_cost_e_fun.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_work;
+    {% else %}
+    capsule->ext_cost_e_fun.fun = &{{ cost.cost_function_ext_cost_e }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun, {{ dims.np }});
+
+    // external cost
+    {% if cost.cost_ext_fun_type_e == "casadi" %}
+    capsule->ext_cost_e_fun_jac.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac;
+    capsule->ext_cost_e_fun_jac.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_in;
+    capsule->ext_cost_e_fun_jac.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_n_out;
+    capsule->ext_cost_e_fun_jac.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in;
+    capsule->ext_cost_e_fun_jac.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out;
+    capsule->ext_cost_e_fun_jac.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_work;
+    {% else %}
+    capsule->ext_cost_e_fun_jac.fun = &{{ cost.cost_function_ext_cost_e }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac, {{ dims.np }});
+
+    // external cost
+    {% if cost.cost_ext_fun_type_e == "casadi" %}
+    capsule->ext_cost_e_fun_jac_hess.casadi_fun = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess;
+    capsule->ext_cost_e_fun_jac_hess.casadi_n_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in;
+    capsule->ext_cost_e_fun_jac_hess.casadi_n_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out;
+    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_in = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in;
+    capsule->ext_cost_e_fun_jac_hess.casadi_sparsity_out = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out;
+    capsule->ext_cost_e_fun_jac_hess.casadi_work = &{{ model.name }}_cost_ext_cost_e_fun_jac_hess_work;
+    {% else %}
+    capsule->ext_cost_e_fun_jac_hess.fun = &{{ cost.cost_function_ext_cost_e }};
+    {% endif %}
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_create(&capsule->ext_cost_e_fun_jac_hess, {{ dims.np }});
+{%- endif %}
+
+    /************************************************
+    *  nlp_in
+    ************************************************/
+    ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims);
+    capsule->nlp_in = nlp_in;
+
+    // set up time_steps
+    {% set all_equal = true -%}
+    {%- set val = solver_options.time_steps[0] %}
+    {%- for j in range(start=1, end=dims.N) %}
+        {%- if val != solver_options.time_steps[j] %}
+            {%- set_global all_equal = false %}
+            {%- break %}
+        {%- endif %}
+    {%- endfor %}
+
+    {%- if all_equal == true -%}
+    // all time_steps are identical
+    double time_step = {{ solver_options.time_steps[0] }};
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step);
+    }
+    {%- else -%}
+    // time_steps are different
+    double* time_steps = malloc(N*sizeof(double));
+    {%- for j in range(end=dims.N) %}
+    time_steps[{{ j }}] = {{ solver_options.time_steps[j] }};
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_steps[i]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_steps[i]);
+    }
+    free(time_steps);
+    {%- endif %}
+
+    /**** Dynamics ****/
+    for (int i = 0; i < N; i++)
+    {
+    {%- if solver_options.integrator_type == "ERK" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_vde_forw", &capsule->forw_vde_casadi[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_fun", &capsule->expl_ode_fun[i]);
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "expl_ode_hess", &capsule->hess_vde_casadi[i]);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "IRK" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i,
+                                   "impl_dae_fun_jac_x_xdot_z", &capsule->impl_dae_fun_jac_x_xdot_z[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i,
+                                   "impl_dae_jac_x_xdot_u", &capsule->impl_dae_jac_x_xdot_u_z[i]);
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_hess", &capsule->impl_dae_hess[i]);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "LIFTED_IRK" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i,
+                                   "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]);
+    {% elif solver_options.integrator_type == "GNSF" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_jac_y_uhat", &capsule->gnsf_phi_jac_y_uhat[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "f_lo_jac_x1_x1dot_u_z",
+                                   &capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "gnsf_get_matrices_fun",
+                                   &capsule->gnsf_get_matrices_fun[i]);
+    {% elif solver_options.integrator_type == "DISCRETE" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun", &capsule->discr_dyn_phi_fun[i]);
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac",
+                                   &capsule->discr_dyn_phi_fun_jac_ut_xt[i]);
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "disc_dyn_fun_jac_hess",
+                                   &capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]);
+        {%- endif %}
+    {%- endif %}
+    }
+
+
+    /**** Cost ****/
+{%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %}
+{% if dims.ny_0 > 0 %}
+    double* W_0 = calloc(NY0*NY0, sizeof(double));
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ny_0) %}
+        {%- for k in range(end=dims.ny_0) %}
+            {%- if cost.W_0[j][k] != 0 %}
+    W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0);
+    free(W_0);
+
+    double* yref_0 = calloc(NY0, sizeof(double));
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ny_0) %}
+        {%- if cost.yref_0[j] != 0 %}
+    yref_0[{{ j }}] = {{ cost.yref_0[j] }};
+        {%- endif %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0);
+    free(yref_0);
+{% endif %}
+{% endif %}
+
+{%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %}
+{% if dims.ny > 0 %}
+    double* W = calloc(NY*NY, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny) %}
+        {%- for k in range(end=dims.ny) %}
+            {%- if cost.W[j][k] != 0 %}
+    W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    double* yref = calloc(NY, sizeof(double));
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ny) %}
+        {%- if cost.yref[j] != 0 %}
+    yref[{{ j }}] = {{ cost.yref[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref);
+    }
+    free(W);
+    free(yref);
+{% endif %}
+{% endif %}
+
+{%- if cost.cost_type_0 == "LINEAR_LS" %}
+    double* Vx_0 = calloc(NY0*NX, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_0) %}
+        {%- for k in range(end=dims.nx) %}
+            {%- if cost.Vx_0[j][k] != 0 %}
+    Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0);
+    free(Vx_0);
+
+{% if dims.ny_0 > 0 and dims.nu > 0 %}
+    double* Vu_0 = calloc(NY0*NU, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_0) %}
+        {%- for k in range(end=dims.nu) %}
+            {%- if cost.Vu_0[j][k] != 0 %}
+    Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0);
+    free(Vu_0);
+{% endif %}
+{% if dims.ny_0 > 0 and dims.nz > 0 %}
+    double* Vz_0 = calloc(NY0*NZ, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_0) %}
+        {%- for k in range(end=dims.nz) %}
+            {%- if cost.Vz_0[j][k] != 0 %}
+    Vz_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vz_0[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0);
+    free(Vz_0);
+{%- endif %}
+{%- endif %}{# LINEAR LS #}
+
+
+{%- if cost.cost_type == "LINEAR_LS" %}
+    double* Vx = calloc(NY*NX, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny) %}
+        {%- for k in range(end=dims.nx) %}
+            {%- if cost.Vx[j][k] != 0 %}
+    Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx);
+    }
+    free(Vx);
+
+{% if dims.ny > 0 and dims.nu > 0 %}
+    double* Vu = calloc(NY*NU, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny) %}
+        {%- for k in range(end=dims.nu) %}
+            {%- if cost.Vu[j][k] != 0 %}
+    Vu[{{ j }}+(NY) * {{ k }}] = {{ cost.Vu[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu);
+    }
+    free(Vu);
+{% endif %}
+
+{% if dims.ny > 0 and dims.nz > 0 %}
+    double* Vz = calloc(NY*NZ, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny) %}
+        {%- for k in range(end=dims.nz) %}
+            {%- if cost.Vz[j][k] != 0 %}
+    Vz[{{ j }}+(NY) * {{ k }}] = {{ cost.Vz[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz);
+    }
+    free(Vz);
+{%- endif %}
+{%- endif %}{# LINEAR LS #}
+
+
+{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun", &capsule->cost_y_0_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_fun_jac", &capsule->cost_y_0_fun_jac_ut_xt);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "nls_y_hess", &capsule->cost_y_0_hess);
+{%- elif cost.cost_type_0 == "EXTERNAL" %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun", &capsule->ext_cost_0_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac", &capsule->ext_cost_0_fun_jac);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "ext_cost_fun_jac_hess", &capsule->ext_cost_0_fun_jac_hess);
+{%- endif %}
+
+{%- if cost.cost_type == "NONLINEAR_LS" %}
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun", &capsule->cost_y_fun[i-1]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_fun_jac", &capsule->cost_y_fun_jac_ut_xt[i-1]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "nls_y_hess", &capsule->cost_y_hess[i-1]);
+    }
+{%- elif cost.cost_type == "EXTERNAL" %}
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun", &capsule->ext_cost_fun[i-1]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac", &capsule->ext_cost_fun_jac[i-1]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "ext_cost_fun_jac_hess", &capsule->ext_cost_fun_jac_hess[i-1]);
+    }
+{%- endif %}
+
+
+{% if dims.ns > 0 %}
+    double* zlumem = calloc(4*NS, sizeof(double));
+    double* Zl = zlumem+NS*0;
+    double* Zu = zlumem+NS*1;
+    double* zl = zlumem+NS*2;
+    double* zu = zlumem+NS*3;
+    // change only the non-zero elements:
+    {%- for j in range(end=dims.ns) %}
+        {%- if cost.Zl[j] != 0 %}
+    Zl[{{ j }}] = {{ cost.Zl[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {%- for j in range(end=dims.ns) %}
+        {%- if cost.Zu[j] != 0 %}
+    Zu[{{ j }}] = {{ cost.Zu[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {%- for j in range(end=dims.ns) %}
+        {%- if cost.zl[j] != 0 %}
+    zl[{{ j }}] = {{ cost.zl[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {%- for j in range(end=dims.ns) %}
+        {%- if cost.zu[j] != 0 %}
+    zu[{{ j }}] = {{ cost.zu[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zl", Zl);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Zu", Zu);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu);
+    }
+    free(zlumem);
+{% endif %}
+
+    // terminal cost
+{% if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %}
+{% if dims.ny_e > 0 %}
+    double* yref_e = calloc(NYN, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_e) %}
+        {%- if cost.yref_e[j] != 0 %}
+    yref_e[{{ j }}] = {{ cost.yref_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e);
+    free(yref_e);
+
+    double* W_e = calloc(NYN*NYN, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_e) %}
+        {%- for k in range(end=dims.ny_e) %}
+            {%- if cost.W_e[j][k] != 0 %}
+    W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e);
+    free(W_e);
+
+    {%- if cost.cost_type_e == "LINEAR_LS" %}
+    double* Vx_e = calloc(NYN*NX, sizeof(double));
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ny_e) %}
+        {%- for k in range(end=dims.nx) %}
+            {%- if cost.Vx_e[j][k] != 0 %}
+    Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e);
+    free(Vx_e);
+    {%- endif %}
+
+    {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun", &capsule->cost_y_e_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_fun_jac", &capsule->cost_y_e_fun_jac_ut_xt);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "nls_y_hess", &capsule->cost_y_e_hess);
+    {%- endif %}
+{%- endif %}{# ny_e > 0 #}
+
+{%- elif cost.cost_type_e == "EXTERNAL" %}
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun", &capsule->ext_cost_e_fun);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac", &capsule->ext_cost_e_fun_jac);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "ext_cost_fun_jac_hess", &capsule->ext_cost_e_fun_jac_hess);
+{%- endif %}
+
+{% if dims.ns_e > 0 %}
+    double* zluemem = calloc(4*NSN, sizeof(double));
+    double* Zl_e = zluemem+NSN*0;
+    double* Zu_e = zluemem+NSN*1;
+    double* zl_e = zluemem+NSN*2;
+    double* zu_e = zluemem+NSN*3;
+
+    // change only the non-zero elements:
+    {% for j in range(end=dims.ns_e) %}
+        {%- if cost.Zl_e[j] != 0 %}
+    Zl_e[{{ j }}] = {{ cost.Zl_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for j in range(end=dims.ns_e) %}
+        {%- if cost.Zu_e[j] != 0 %}
+    Zu_e[{{ j }}] = {{ cost.Zu_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for j in range(end=dims.ns_e) %}
+        {%- if cost.zl_e[j] != 0 %}
+    zl_e[{{ j }}] = {{ cost.zl_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for j in range(end=dims.ns_e) %}
+        {%- if cost.zu_e[j] != 0 %}
+    zu_e[{{ j }}] = {{ cost.zu_e[j] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e);
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e);
+    free(zluemem);
+{%- endif %}
+
+    /**** Constraints ****/
+
+    // bounds for initial stage
+{% if dims.nbx_0 > 0 %}
+    // x0
+    int* idxbx0 = malloc(NBX0 * sizeof(int));
+    {%- for i in range(end=dims.nbx_0) %}
+    idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
+    {%- endfor %}
+
+    double* lubx0 = calloc(2*NBX0, sizeof(double));
+    double* lbx0 = lubx0;
+    double* ubx0 = lubx0 + NBX0;
+    // change only the non-zero elements:
+    {%- for i in range(end=dims.nbx_0) %}
+        {%- if constraints.lbx_0[i] != 0 %}
+    lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
+        {%- endif %}
+        {%- if constraints.ubx_0[i] != 0 %}
+    ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+    free(idxbx0);
+    free(lubx0);
+{% endif %}
+{% if dims.nbxe_0 > 0 %}
+    // idxbxe_0
+    int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int));
+    {% for i in range(end=dims.nbxe_0) %}
+    idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }};
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0);
+    free(idxbxe_0);
+{% endif %}
+
+    /* constraints that are the same for initial and intermediate */
+{%- if dims.nsbx > 0 %}
+{# TODO: introduce nsbx0 & move this block down!! #}
+    // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx);
+    // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx);
+    // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx);
+
+    // soft bounds on x
+    int* idxsbx = malloc(NSBX * sizeof(int));
+    {% for i in range(end=dims.nsbx) %}
+    idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }};
+    {%- endfor %}
+
+    double* lusbx = calloc(2*NSBX, sizeof(double));
+    double* lsbx = lusbx;
+    double* usbx = lusbx + NSBX;
+    {% for i in range(end=dims.nsbx) %}
+        {%- if constraints.lsbx[i] != 0 %}
+    lsbx[{{ i }}] = {{ constraints.lsbx[i] }};
+        {%- endif %}
+        {%- if constraints.usbx[i] != 0 %}
+    usbx[{{ i }}] = {{ constraints.usbx[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {       
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbx", idxsbx);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx);
+    }
+    free(idxsbx);
+    free(lusbx);
+{%- endif %}
+
+
+{% if dims.nbu > 0 %}
+    // u
+    int* idxbu = malloc(NBU * sizeof(int));
+    {% for i in range(end=dims.nbu) %}
+    idxbu[{{ i }}] = {{ constraints.idxbu[i] }};
+    {%- endfor %}
+    double* lubu = calloc(2*NBU, sizeof(double));
+    double* lbu = lubu;
+    double* ubu = lubu + NBU;
+    {% for i in range(end=dims.nbu) %}
+        {%- if constraints.lbu[i] != 0 %}
+    lbu[{{ i }}] = {{ constraints.lbu[i] }};
+        {%- endif %}
+        {%- if constraints.ubu[i] != 0 %}
+    ubu[{{ i }}] = {{ constraints.ubu[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbu", idxbu);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu);
+    }
+    free(idxbu);
+    free(lubu);
+{% endif %}
+
+{% if dims.nsbu > 0 %}
+    // set up soft bounds for u
+    int* idxsbu = malloc(NSBU * sizeof(int));
+    {% for i in range(end=dims.nsbu) %}
+    idxsbu[{{ i }}] = {{ constraints.idxsbu[i] }};
+    {%- endfor %}
+    double* lusbu = calloc(2*NSBU, sizeof(double));
+    double* lsbu = lusbu;
+    double* usbu = lusbu + NSBU;
+    {% for i in range(end=dims.nsbu) %}
+        {%- if constraints.lsbu[i] != 0 %}
+    lsbu[{{ i }}] = {{ constraints.lsbu[i] }};
+        {%- endif %}
+        {%- if constraints.usbu[i] != 0 %}
+    usbu[{{ i }}] = {{ constraints.usbu[i] }};
+        {%- endif %}
+    {%- endfor %}
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsbu", idxsbu);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu);
+    }
+    free(idxsbu);
+    free(lusbu);
+{% endif %}
+
+{% if dims.nsg > 0 %}
+    // set up soft bounds for general linear constraints
+    int* idxsg = malloc(NSG * sizeof(int));
+    {% for i in range(end=dims.nsg) %}
+    idxsg[{{ i }}] = {{ constraints.idxsg[i] }};
+    {%- endfor %}
+    double* lusg = calloc(2*NSG, sizeof(double));
+    double* lsg = lusg;
+    double* usg = lusg + NSG;
+    {% for i in range(end=dims.nsg) %}
+        {%- if constraints.lsg[i] != 0 %}
+    lsg[{{ i }}] = {{ constraints.lsg[i] }};
+        {%- endif %}
+        {%- if constraints.usg[i] != 0 %}
+    usg[{{ i }}] = {{ constraints.usg[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsg", idxsg);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg);
+    }
+    free(idxsg);
+    free(lusg);
+{% endif %}
+
+{% if dims.nsh > 0 %}
+    // set up soft bounds for nonlinear constraints
+    int* idxsh = malloc(NSH * sizeof(int));
+    {% for i in range(end=dims.nsh) %}
+    idxsh[{{ i }}] = {{ constraints.idxsh[i] }};
+    {%- endfor %}
+    double* lush = calloc(2*NSH, sizeof(double));
+    double* lsh = lush;
+    double* ush = lush + NSH;
+    {% for i in range(end=dims.nsh) %}
+        {%- if constraints.lsh[i] != 0 %}
+    lsh[{{ i }}] = {{ constraints.lsh[i] }};
+        {%- endif %}
+        {%- if constraints.ush[i] != 0 %}
+    ush[{{ i }}] = {{ constraints.ush[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsh", idxsh);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush);
+    }
+    free(idxsh);
+    free(lush);
+{% endif %}
+
+{% if dims.nsphi > 0 %}
+    // set up soft bounds for convex-over-nonlinear constraints
+    int* idxsphi = malloc(NSPHI * sizeof(int));
+    {% for i in range(end=dims.nsphi) %}
+    idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }};
+    {%- endfor %}
+    double* lusphi = calloc(2*NSPHI, sizeof(double));
+    double* lsphi = lusphi;
+    double* usphi = lusphi + NSPHI;
+    {% for i in range(end=dims.nsphi) %}
+        {%- if constraints.lsphi[i] != 0 %}
+    lsphi[{{ i }}] = {{ constraints.lsphi[i] }};
+        {%- endif %}
+        {%- if constraints.usphi[i] != 0 %}
+    usphi[{{ i }}] = {{ constraints.usphi[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxsphi", idxsphi);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi);
+    }
+    free(idxsphi);
+    free(lusphi);
+{% endif %}
+
+{% if dims.nbx > 0 %}
+    // x
+    int* idxbx = malloc(NBX * sizeof(int));
+    {% for i in range(end=dims.nbx) %}
+    idxbx[{{ i }}] = {{ constraints.idxbx[i] }};
+    {%- endfor %}
+    double* lubx = calloc(2*NBX, sizeof(double));
+    double* lbx = lubx;
+    double* ubx = lubx + NBX;
+    {% for i in range(end=dims.nbx) %}
+        {%- if constraints.lbx[i] != 0 %}
+    lbx[{{ i }}] = {{ constraints.lbx[i] }};
+        {%- endif %}
+        {%- if constraints.ubx[i] != 0 %}
+    ubx[{{ i }}] = {{ constraints.ubx[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 1; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "idxbx", idxbx);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx);
+    }
+    free(idxbx);
+    free(lubx);
+{% endif %}
+
+{% if dims.ng > 0 %}
+    // set up general constraints for stage 0 to N-1 
+    double* D = calloc(NG*NU, sizeof(double));
+    double* C = calloc(NG*NX, sizeof(double));
+    double* lug = calloc(2*NG, sizeof(double));
+    double* lg = lug;
+    double* ug = lug + NG;
+
+    {% for j in range(end=dims.ng) -%}
+        {% for k in range(end=dims.nu) %}
+            {%- if constraints.D[j][k] != 0 %}
+    D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    {% for j in range(end=dims.ng) -%}
+        {% for k in range(end=dims.nx) %}
+            {%- if constraints.C[j][k] != 0 %}
+    C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.ng) %}
+        {%- if constraints.lg[i] != 0 %}
+    lg[{{ i }}] = {{ constraints.lg[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.ng) %}
+        {%- if constraints.ug[i] != 0 %}
+    ug[{{ i }}] = {{ constraints.ug[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "D", D);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "C", C);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug);
+    }
+    free(D);
+    free(C);
+    free(lug);
+{% endif %}
+
+{% if dims.nh > 0 %}
+    // set up nonlinear constraints for stage 0 to N-1
+    double* luh = calloc(2*NH, sizeof(double));
+    double* lh = luh;
+    double* uh = luh + NH;
+
+    {% for i in range(end=dims.nh) %}
+        {%- if constraints.lh[i] != 0 %}
+    lh[{{ i }}] = {{ constraints.lh[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.nh) %}
+        {%- if constraints.uh[i] != 0 %}
+    uh[{{ i }}] = {{ constraints.uh[i] }};
+        {%- endif %}
+    {%- endfor %}
+    
+    for (int i = 0; i < N; i++)
+    {
+        // nonlinear constraints for stages 0 to N-1
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun_jac",
+                                      &capsule->nl_constr_h_fun_jac[i]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "nl_constr_h_fun",
+                                      &capsule->nl_constr_h_fun[i]);
+        {% if solver_options.hessian_approx == "EXACT" %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i,
+                                      "nl_constr_h_fun_jac_hess", &capsule->nl_constr_h_fun_jac_hess[i]);
+        {% endif %}
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh);
+    }
+    free(luh);
+{% endif %}
+
+{% if dims.nphi > 0 and constraints.constr_type == "BGP" %}
+    // set up convex-over-nonlinear constraints for stage 0 to N-1
+    double* luphi = calloc(2*NPHI, sizeof(double));
+    double* lphi = luphi;
+    double* uphi = luphi + NPHI;
+    {% for i in range(end=dims.nphi) %}
+        {%- if constraints.lphi[i] != 0 %}
+    lphi[{{ i }}] = {{ constraints.lphi[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.nphi) %}
+        {%- if constraints.uphi[i] != 0 %}
+    uphi[{{ i }}] = {{ constraints.uphi[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i,
+                                      "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_constraint[i]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi);
+    }
+    free(luphi);
+{% endif %}
+
+    /* terminal constraints */
+{% if dims.nbx_e > 0 %}
+    // set up bounds for last stage
+    // x
+    int* idxbx_e = malloc(NBXN * sizeof(int));
+    {% for i in range(end=dims.nbx_e) %}
+    idxbx_e[{{ i }}] = {{ constraints.idxbx_e[i] }};
+    {%- endfor %}
+    double* lubx_e = calloc(2*NBXN, sizeof(double));
+    double* lbx_e = lubx_e;
+    double* ubx_e = lubx_e + NBXN;
+    {% for i in range(end=dims.nbx_e) %}
+        {%- if constraints.lbx_e[i] != 0 %}
+    lbx_e[{{ i }}] = {{ constraints.lbx_e[i] }};
+        {%- endif %}
+        {%- if constraints.ubx_e[i] != 0 %}
+    ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e);
+    free(idxbx_e);
+    free(lubx_e);
+{%- endif %}
+
+{% if dims.nsg_e > 0 %}
+    // set up soft bounds for general linear constraints
+    int* idxsg_e = calloc(NSGN, sizeof(int));
+    {% for i in range(end=dims.nsg_e) %}
+    idxsg_e[{{ i }}] = {{ constraints.idxsg_e[i] }};
+    {%- endfor %}
+    double* lusg_e = calloc(2*NSGN, sizeof(double));
+    double* lsg_e = lusg_e;
+    double* usg_e = lusg_e + NSGN;
+    {% for i in range(end=dims.nsg_e) %}
+        {%- if constraints.lsg_e[i] != 0 %}
+    lsg_e[{{ i }}] = {{ constraints.lsg_e[i] }};
+        {%- endif %}
+        {%- if constraints.usg_e[i] != 0 %}
+    usg_e[{{ i }}] = {{ constraints.usg_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e);
+    free(idxsg_e);
+    free(lusg_e);
+{%- endif %}
+
+{% if dims.nsh_e > 0 %}
+    // set up soft bounds for nonlinear constraints
+    int* idxsh_e = malloc(NSHN * sizeof(int));
+    {% for i in range(end=dims.nsh_e) %}
+    idxsh_e[{{ i }}] = {{ constraints.idxsh_e[i] }};
+    {%- endfor %}
+    double* lush_e = calloc(2*NSHN, sizeof(double));
+    double* lsh_e = lush_e;
+    double* ush_e = lush_e + NSHN;
+    {% for i in range(end=dims.nsh_e) %}
+        {%- if constraints.lsh_e[i] != 0 %}
+    lsh_e[{{ i }}] = {{ constraints.lsh_e[i] }};
+        {%- endif %}
+        {%- if constraints.ush_e[i] != 0 %}
+    ush_e[{{ i }}] = {{ constraints.ush_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e);
+    free(idxsh_e);
+    free(lush_e);
+{%- endif %}
+
+{% if dims.nsphi_e > 0 %}
+    // set up soft bounds for convex-over-nonlinear constraints
+    int* idxsphi_e = malloc(NSPHIN * sizeof(int));
+    {% for i in range(end=dims.nsphi_e) %}
+    idxsphi_e[{{ i }}] = {{ constraints.idxsphi_e[i] }};
+    {%- endfor %}
+    double* lusphi_e = calloc(2*NSPHIN, sizeof(double));
+    double* lsphi_e = lusphi_e;
+    double* usphi_e = lusphi_e + NSPHIN;
+    {% for i in range(end=dims.nsphi_e) %}
+        {%- if constraints.lsphi_e[i] != 0 %}
+    lsphi_e[{{ i }}] = {{ constraints.lsphi_e[i] }};
+        {%- endif %}
+        {%- if constraints.usphi_e[i] != 0 %}
+    usphi_e[{{ i }}] = {{ constraints.usphi_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e);
+    free(idxsphi_e);
+    free(lusphi_e);
+{%- endif %}
+
+{% if dims.nsbx_e > 0 %}
+    // soft bounds on x
+    int* idxsbx_e = malloc(NSBXN * sizeof(int));
+    {% for i in range(end=dims.nsbx_e) %}
+    idxsbx_e[{{ i }}] = {{ constraints.idxsbx_e[i] }};
+    {%- endfor %}
+    double* lusbx_e = calloc(2*NSBXN, sizeof(double));
+    double* lsbx_e = lusbx_e;
+    double* usbx_e = lusbx_e + NSBXN;
+    {% for i in range(end=dims.nsbx_e) %}
+        {%- if constraints.lsbx_e[i] != 0 %}
+    lsbx_e[{{ i }}] = {{ constraints.lsbx_e[i] }};
+        {%- endif %}
+        {%- if constraints.usbx_e[i] != 0 %}
+    usbx_e[{{ i }}] = {{ constraints.usbx_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e);
+    free(idxsbx_e);
+    free(lusbx_e);
+{% endif %}
+
+{% if dims.ng_e > 0 %}
+    // set up general constraints for last stage 
+    double* C_e = calloc(NGN*NX, sizeof(double));
+    double* lug_e = calloc(2*NGN, sizeof(double));
+    double* lg_e = lug_e;
+    double* ug_e = lug_e + NGN;
+
+    {% for j in range(end=dims.ng) %}
+        {%- for k in range(end=dims.nx) %}
+            {%- if constraints.C_e[j][k] != 0 %}
+    C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }};
+            {%- endif %}
+        {%- endfor %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.ng_e) %}
+        {%- if constraints.lg_e[i] != 0 %}
+    lg_e[{{ i }}] = {{ constraints.lg_e[i] }};
+        {%- endif %}
+        {%- if constraints.ug_e[i] != 0 %}
+    ug_e[{{ i }}] = {{ constraints.ug_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e);
+    free(C_e);
+    free(lug_e);
+{%- endif %}
+
+{% if dims.nh_e > 0 %}
+    // set up nonlinear constraints for last stage
+    double* luh_e = calloc(2*NHN, sizeof(double));
+    double* lh_e = luh_e;
+    double* uh_e = luh_e + NHN;
+    {% for i in range(end=dims.nh_e) %}
+        {%- if constraints.lh_e[i] != 0 %}
+    lh_e[{{ i }}] = {{ constraints.lh_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    {% for i in range(end=dims.nh_e) %}
+        {%- if constraints.uh_e[i] != 0 %}
+    uh_e[{{ i }}] = {{ constraints.uh_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun", &capsule->nl_constr_h_e_fun);
+    {% if solver_options.hessian_approx == "EXACT" %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac_hess",
+                                  &capsule->nl_constr_h_e_fun_jac_hess);
+    {% endif %}
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e);
+    free(luh_e);
+{%- endif %}
+
+{% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %}
+    // set up convex-over-nonlinear constraints for last stage 
+    double* luphi_e = calloc(2*NPHIN, sizeof(double));
+    double* lphi_e = luphi_e;
+    double* uphi_e = luphi_e + NPHIN;
+    {% for i in range(end=dims.nphi_e) %}
+        {%- if constraints.lphi_e[i] != 0 %}
+    lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }};
+        {%- endif %}
+        {%- if constraints.uphi_e[i] != 0 %}
+    uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }};
+        {%- endif %}
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N,
+                                  "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint);
+    free(luphi_e);
+{% endif %}
+
+
+    /************************************************
+    *  opts
+    ************************************************/
+
+    capsule->nlp_opts = ocp_nlp_solver_opts_create(nlp_config, nlp_dims);
+
+{% if solver_options.hessian_approx == "EXACT" %}
+    bool nlp_solver_exact_hessian = true;
+    // TODO: this if should not be needed! however, calling the setter with false leads to weird behavior. Investigate!
+    if (nlp_solver_exact_hessian)
+    {
+        ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess", &nlp_solver_exact_hessian);
+    }
+    int exact_hess_dyn = {{ solver_options.exact_hess_dyn }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_dyn", &exact_hess_dyn);
+
+    int exact_hess_cost = {{ solver_options.exact_hess_cost }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_cost", &exact_hess_cost);
+
+    int exact_hess_constr = {{ solver_options.exact_hess_constr }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "exact_hess_constr", &exact_hess_constr);
+{%- endif -%}
+
+{%- if solver_options.globalization == "FIXED_STEP" %}
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "fixed_step");
+{%- elif solver_options.globalization == "MERIT_BACKTRACKING" %}
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "globalization", "merit_backtracking");
+
+    double alpha_min = {{ solver_options.alpha_min }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_min", &alpha_min);
+
+    double alpha_reduction = {{ solver_options.alpha_reduction }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "alpha_reduction", &alpha_reduction);
+{%- endif -%}
+
+{%- if dims.nz > 0 %}
+    // TODO: these options are lower level -> should be encapsulated! maybe through hessian approx option.
+    bool output_z_val = true;
+    bool sens_algebraic_val = true;
+
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_output_z", &output_z_val);
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_sens_algebraic", &sens_algebraic_val);
+{%- endif %}
+
+{%- if solver_options.integrator_type != "DISCRETE" %}
+
+    // set up sim_method_num_steps
+    {%- set all_equal = true %}
+    {%- set val = solver_options.sim_method_num_steps[0] %}
+    {%- for j in range(start=1, end=dims.N) %}
+        {%- if val != solver_options.sim_method_num_steps[j] %}
+            {%- set_global all_equal = false %}
+            {%- break %}
+        {%- endif %}
+    {%- endfor %}
+
+    {%- if all_equal == true %}
+    // all sim_method_num_steps are identical
+    int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }};
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps);
+    {%- else %}
+    // sim_method_num_steps are different
+    int* sim_method_num_steps = malloc(N*sizeof(int));
+    {%- for j in range(end=dims.N) %}
+    sim_method_num_steps[{{ j }}] = {{ solver_options.sim_method_num_steps[j] }};
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]);
+    free(sim_method_num_steps);
+    {%- endif %}
+
+    // set up sim_method_num_stages
+    {%- set all_equal = true %}
+    {%- set val = solver_options.sim_method_num_stages[0] %}
+    {%- for j in range(start=1, end=dims.N) %}
+        {%- if val != solver_options.sim_method_num_stages[j] %}
+            {%- set_global all_equal = false %}
+            {%- break %}
+        {%- endif %}
+    {%- endfor %}
+
+  {%- if all_equal == true %}
+    // all sim_method_num_stages are identical
+    int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }};
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages);
+  {%- else %}
+    int* sim_method_num_stages = malloc(N*sizeof(int));
+    {%- for j in range(end=dims.N) %}
+    sim_method_num_stages[{{ j }}] = {{ solver_options.sim_method_num_stages[j] }};
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]);
+    free(sim_method_num_stages);
+  {%- endif %}
+
+    int newton_iter_val = {{ solver_options.sim_method_newton_iter }};
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_newton_iter", &newton_iter_val);
+
+
+    // set up sim_method_jac_reuse
+    {%- set all_equal = true %}
+    {%- set val = solver_options.sim_method_jac_reuse[0] %}
+    {%- for j in range(start=1, end=dims.N) %}
+        {%- if val != solver_options.sim_method_jac_reuse[j] %}
+            {%- set_global all_equal = false %}
+            {%- break %}
+        {%- endif %}
+    {%- endfor %}
+  {%- if all_equal == true %}
+    bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }};
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &tmp_bool);
+  {%- else %}
+    bool* sim_method_jac_reuse = malloc(N*sizeof(bool));
+    {%- for j in range(end=dims.N) %}
+    sim_method_jac_reuse[{{ j }}] = (bool){{ solver_options.sim_method_jac_reuse[j] }};
+    {%- endfor %}
+
+    for (int i = 0; i < N; i++)
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]);
+    free(sim_method_jac_reuse);
+  {%- endif %}
+
+{%- endif %}
+
+    double nlp_solver_step_length = {{ solver_options.nlp_solver_step_length }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "step_length", &nlp_solver_step_length);
+
+    {%- if solver_options.nlp_solver_warm_start_first_qp %}
+    int nlp_solver_warm_start_first_qp = {{ solver_options.nlp_solver_warm_start_first_qp }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "warm_start_first_qp", &nlp_solver_warm_start_first_qp);
+    {%- endif %}
+
+    double levenberg_marquardt = {{ solver_options.levenberg_marquardt }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "levenberg_marquardt", &levenberg_marquardt);
+
+    /* options QP solver */
+{%- if solver_options.qp_solver is starting_with("PARTIAL_CONDENSING") %}
+    int qp_solver_cond_N;
+
+    {%- if solver_options.qp_solver_cond_N %}
+    qp_solver_cond_N = {{ solver_options.qp_solver_cond_N }};
+    {% else %}
+    // NOTE: there is no condensing happening here!
+    qp_solver_cond_N = N;
+    {%- endif %}
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_cond_N", &qp_solver_cond_N);
+{% endif %}
+
+    int qp_solver_iter_max = {{ solver_options.qp_solver_iter_max }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_iter_max", &qp_solver_iter_max);
+
+    {%- if solver_options.qp_solver_tol_stat %}
+    double qp_solver_tol_stat = {{ solver_options.qp_solver_tol_stat }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_stat", &qp_solver_tol_stat);
+    {%- endif -%}
+
+    {%- if solver_options.qp_solver_tol_eq %}
+    double qp_solver_tol_eq = {{ solver_options.qp_solver_tol_eq }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_eq", &qp_solver_tol_eq);
+    {%- endif -%}
+
+    {%- if solver_options.qp_solver_tol_ineq %}
+    double qp_solver_tol_ineq = {{ solver_options.qp_solver_tol_ineq }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_ineq", &qp_solver_tol_ineq);
+    {%- endif -%}
+
+    {%- if solver_options.qp_solver_tol_comp %}
+    double qp_solver_tol_comp = {{ solver_options.qp_solver_tol_comp }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_tol_comp", &qp_solver_tol_comp);
+    {%- endif -%}
+
+    {%- if solver_options.qp_solver_warm_start %}
+    int qp_solver_warm_start = {{ solver_options.qp_solver_warm_start }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "qp_warm_start", &qp_solver_warm_start);
+    {%- endif -%}
+    
+{% if solver_options.nlp_solver_type == "SQP" %}
+    // set SQP specific options
+    double nlp_solver_tol_stat = {{ solver_options.nlp_solver_tol_stat }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_stat", &nlp_solver_tol_stat);
+
+    double nlp_solver_tol_eq = {{ solver_options.nlp_solver_tol_eq }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_eq", &nlp_solver_tol_eq);
+
+    double nlp_solver_tol_ineq = {{ solver_options.nlp_solver_tol_ineq }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_ineq", &nlp_solver_tol_ineq);
+
+    double nlp_solver_tol_comp = {{ solver_options.nlp_solver_tol_comp }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "tol_comp", &nlp_solver_tol_comp);
+
+    int nlp_solver_max_iter = {{ solver_options.nlp_solver_max_iter }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "max_iter", &nlp_solver_max_iter);
+
+    int initialize_t_slacks = {{ solver_options.initialize_t_slacks }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "initialize_t_slacks", &initialize_t_slacks);
+{%- endif %}
+
+    int print_level = {{ solver_options.print_level }};
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "print_level", &print_level);
+
+
+    int ext_cost_num_hess = {{ solver_options.ext_cost_num_hess }};
+{%- if cost.cost_type == "EXTERNAL" %}
+    for (int i = 0; i < N; i++)
+    {
+        ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "cost_numerical_hessian", &ext_cost_num_hess);
+    }
+{%- endif %}
+{%- if cost.cost_type_e == "EXTERNAL" %}
+    ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, N, "cost_numerical_hessian", &ext_cost_num_hess);
+{%- endif %}
+
+
+    /* out */
+    ocp_nlp_out * nlp_out = ocp_nlp_out_create(nlp_config, nlp_dims);
+    capsule->nlp_out = nlp_out;
+
+    // initialize primal solution
+    double* xu0 = calloc(NX+NU, sizeof(double));
+    double* x0 = xu0;
+{% if dims.nbx_0 == dims.nx %}
+    // initialize with x0
+    {% for item in constraints.lbx_0 %}
+        {%- if item != 0 %}
+    x0[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+{% else %}
+    // initialize with zeros
+{%- endif %}
+
+    double* u0 = xu0 + NX;
+
+    for (int i = 0; i < N; i++)
+    {
+        // x0
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x0);
+        // u0
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+    }
+    ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x0);
+    free(xu0);
+    
+    capsule->nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, capsule->nlp_opts);
+
+
+{% if dims.np > 0 %}
+    // initialize parameters to nominal value
+    double* p = calloc(NP, sizeof(double));
+    {% for item in parameter_values %}
+        {%- if item != 0 %}
+    p[{{ loop.index0 }}] = {{ item }};
+        {%- endif %}
+    {%- endfor %}
+
+    for (int i = 0; i <= N; i++)
+    {
+        {{ model.name }}_acados_update_params(capsule, i, p, NP);
+    }
+    free(p);
+{%- endif %}{# if dims.np #}
+
+    status = ocp_nlp_precompute(capsule->nlp_solver, nlp_in, nlp_out);
+
+    if (status != ACADOS_SUCCESS)
+    {
+        printf("\nocp_precompute failed!\n\n");
+        exit(1);
+    }
+
+    return status;
+}
+
+
+int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *p, int np)
+{
+    int solver_status = 0;
+
+    int casadi_np = {{ dims.np }};
+    if (casadi_np != np) {
+        printf("acados_update_params: trying to set %i parameters for external functions."
+            " External function has %i parameters. Exiting.\n", np, casadi_np);
+        exit(1);
+    }
+
+{%- if dims.np > 0 %}
+    if (stage < {{ dims.N }} && stage >= 0)
+    {
+    {%- if solver_options.integrator_type == "IRK" %}
+        capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p);
+        capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p);
+        capsule->impl_dae_jac_x_xdot_u_z[stage].set_param(capsule->impl_dae_jac_x_xdot_u_z+stage, p);
+
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->impl_dae_hess[stage].set_param(capsule->impl_dae_hess+stage, p);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "LIFTED_IRK" %}
+        capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p);
+        capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p);
+    {% elif solver_options.integrator_type == "ERK" %}
+        capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p);
+        capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p);
+
+        {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->hess_vde_casadi[stage].set_param(capsule->hess_vde_casadi+stage, p);
+        {%- endif %}
+    {% elif solver_options.integrator_type == "GNSF" %}
+        capsule->gnsf_phi_fun[stage].set_param(capsule->gnsf_phi_fun+stage, p);
+        capsule->gnsf_phi_fun_jac_y[stage].set_param(capsule->gnsf_phi_fun_jac_y+stage, p);
+        capsule->gnsf_phi_jac_y_uhat[stage].set_param(capsule->gnsf_phi_jac_y_uhat+stage, p);
+
+        capsule->gnsf_f_lo_jac_x1_x1dot_u_z[stage].set_param(capsule->gnsf_f_lo_jac_x1_x1dot_u_z+stage, p);
+    {% elif solver_options.integrator_type == "DISCRETE" %}
+        capsule->discr_dyn_phi_fun[stage].set_param(capsule->discr_dyn_phi_fun+stage, p);
+        capsule->discr_dyn_phi_fun_jac_ut_xt[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt+stage, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->discr_dyn_phi_fun_jac_ut_xt_hess[stage].set_param(capsule->discr_dyn_phi_fun_jac_ut_xt_hess+stage, p);
+    {% endif %}
+    {%- endif %}{# integrator_type #}
+
+        // constraints
+    {% if constraints.constr_type == "BGP" %}
+        capsule->phi_constraint[stage].set_param(capsule->phi_constraint+stage, p);
+    {% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
+        capsule->nl_constr_h_fun_jac[stage].set_param(capsule->nl_constr_h_fun_jac+stage, p);
+        capsule->nl_constr_h_fun[stage].set_param(capsule->nl_constr_h_fun+stage, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->nl_constr_h_fun_jac_hess[stage].set_param(capsule->nl_constr_h_fun_jac_hess+stage, p);
+    {%- endif %}
+    {%- endif %}
+
+        // cost
+        if (stage == 0)
+        {
+        {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+            capsule->cost_y_0_fun.set_param(&capsule->cost_y_0_fun, p);
+            capsule->cost_y_0_fun_jac_ut_xt.set_param(&capsule->cost_y_0_fun_jac_ut_xt, p);
+            capsule->cost_y_0_hess.set_param(&capsule->cost_y_0_hess, p);
+        {%- elif cost.cost_type_0 == "EXTERNAL" %}
+            capsule->ext_cost_0_fun.set_param(&capsule->ext_cost_0_fun, p);
+            capsule->ext_cost_0_fun_jac.set_param(&capsule->ext_cost_0_fun_jac, p);
+            capsule->ext_cost_0_fun_jac_hess.set_param(&capsule->ext_cost_0_fun_jac_hess, p);
+        {% endif %}
+        }
+        else // 0 < stage < N
+        {
+        {%- if cost.cost_type == "NONLINEAR_LS" %}
+            capsule->cost_y_fun[stage-1].set_param(capsule->cost_y_fun+stage-1, p);
+            capsule->cost_y_fun_jac_ut_xt[stage-1].set_param(capsule->cost_y_fun_jac_ut_xt+stage-1, p);
+            capsule->cost_y_hess[stage-1].set_param(capsule->cost_y_hess+stage-1, p);
+        {%- elif cost.cost_type == "EXTERNAL" %}
+            capsule->ext_cost_fun[stage-1].set_param(capsule->ext_cost_fun+stage-1, p);
+            capsule->ext_cost_fun_jac[stage-1].set_param(capsule->ext_cost_fun_jac+stage-1, p);
+            capsule->ext_cost_fun_jac_hess[stage-1].set_param(capsule->ext_cost_fun_jac_hess+stage-1, p);
+        {%- endif %}
+        }
+    }
+
+    else // stage == N
+    {
+        // terminal shooting node has no dynamics
+        // cost
+    {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+        capsule->cost_y_e_fun.set_param(&capsule->cost_y_e_fun, p);
+        capsule->cost_y_e_fun_jac_ut_xt.set_param(&capsule->cost_y_e_fun_jac_ut_xt, p);
+        capsule->cost_y_e_hess.set_param(&capsule->cost_y_e_hess, p);
+    {%- elif cost.cost_type_e == "EXTERNAL" %}
+        capsule->ext_cost_e_fun.set_param(&capsule->ext_cost_e_fun, p);
+        capsule->ext_cost_e_fun_jac.set_param(&capsule->ext_cost_e_fun_jac, p);
+        capsule->ext_cost_e_fun_jac_hess.set_param(&capsule->ext_cost_e_fun_jac_hess, p);
+    {% endif %}
+        // constraints
+    {% if constraints.constr_type_e == "BGP" %}
+        capsule->phi_e_constraint.set_param(&capsule->phi_e_constraint, p);
+    {% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+        capsule->nl_constr_h_e_fun_jac.set_param(&capsule->nl_constr_h_e_fun_jac, p);
+        capsule->nl_constr_h_e_fun.set_param(&capsule->nl_constr_h_e_fun, p);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        capsule->nl_constr_h_e_fun_jac_hess.set_param(&capsule->nl_constr_h_e_fun_jac_hess, p);
+    {%- endif %}
+    {% endif %}
+    }
+{% endif %}{# if dims.np #}
+
+    return solver_status;
+}
+
+
+
+int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule)
+{
+    // solve NLP 
+    int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
+
+    return solver_status;
+}
+
+
+int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
+{
+    // free memory
+    ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
+    ocp_nlp_in_destroy(capsule->nlp_in);
+    ocp_nlp_out_destroy(capsule->nlp_out);
+    ocp_nlp_solver_destroy(capsule->nlp_solver);
+    ocp_nlp_dims_destroy(capsule->nlp_dims);
+    ocp_nlp_config_destroy(capsule->nlp_config);
+    ocp_nlp_plan_destroy(capsule->nlp_solver_plan);
+
+    /* free external function */
+    // dynamics
+{%- if solver_options.integrator_type == "IRK" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
+        external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
+        external_function_param_casadi_free(&capsule->impl_dae_jac_x_xdot_u_z[i]);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        external_function_param_casadi_free(&capsule->impl_dae_hess[i]);
+    {%- endif %}
+    }
+    free(capsule->impl_dae_fun);
+    free(capsule->impl_dae_fun_jac_x_xdot_z);
+    free(capsule->impl_dae_jac_x_xdot_u_z);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    free(capsule->impl_dae_hess);
+    {%- endif %}
+
+{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
+        external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]);
+    }
+    free(capsule->impl_dae_fun);
+    free(capsule->impl_dae_fun_jac_x_xdot_u);
+
+{%- elif solver_options.integrator_type == "ERK" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->forw_vde_casadi[i]);
+        external_function_param_casadi_free(&capsule->expl_ode_fun[i]);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        external_function_param_casadi_free(&capsule->hess_vde_casadi[i]);
+    {%- endif %}
+    }
+    free(capsule->forw_vde_casadi);
+    free(capsule->expl_ode_fun);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    free(capsule->hess_vde_casadi);
+    {%- endif %}
+
+{%- elif solver_options.integrator_type == "GNSF" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]);
+        external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]);
+        external_function_param_casadi_free(&capsule->gnsf_phi_jac_y_uhat[i]);
+        external_function_param_casadi_free(&capsule->gnsf_f_lo_jac_x1_x1dot_u_z[i]);
+        external_function_param_casadi_free(&capsule->gnsf_get_matrices_fun[i]);
+    }
+    free(capsule->gnsf_phi_fun);
+    free(capsule->gnsf_phi_fun_jac_y);
+    free(capsule->gnsf_phi_jac_y_uhat);
+    free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z);
+    free(capsule->gnsf_get_matrices_fun);
+{%- elif solver_options.integrator_type == "DISCRETE" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]);
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+        external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt_hess[i]);
+    {%- endif %}
+    }
+    free(capsule->discr_dyn_phi_fun);
+    free(capsule->discr_dyn_phi_fun_jac_ut_xt);
+    {%- if solver_options.hessian_approx == "EXACT" %}
+    free(capsule->discr_dyn_phi_fun_jac_ut_xt_hess);
+    {%- endif %}
+    
+{%- endif %}
+
+    // cost
+{%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+    external_function_param_casadi_free(&capsule->cost_y_0_fun);
+    external_function_param_casadi_free(&capsule->cost_y_0_fun_jac_ut_xt);
+    external_function_param_casadi_free(&capsule->cost_y_0_hess);
+{%- elif cost.cost_type_0 == "EXTERNAL" %}
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun);
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac);
+    external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess);
+{%- endif %}
+{%- if cost.cost_type == "NONLINEAR_LS" %}
+    for (int i = 0; i < {{ dims.N }} - 1; i++)
+    {
+        external_function_param_casadi_free(&capsule->cost_y_fun[i]);
+        external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]);
+        external_function_param_casadi_free(&capsule->cost_y_hess[i]);
+    }
+    free(capsule->cost_y_fun);
+    free(capsule->cost_y_fun_jac_ut_xt);
+    free(capsule->cost_y_hess);
+{%- elif cost.cost_type == "EXTERNAL" %}
+    for (int i = 0; i < {{ dims.N }} - 1; i++)
+    {
+        external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]);
+        external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]);
+        external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac_hess[i]);
+    }
+    free(capsule->ext_cost_fun);
+    free(capsule->ext_cost_fun_jac);
+    free(capsule->ext_cost_fun_jac_hess);
+{%- endif %}
+{%- if cost.cost_type_e == "NONLINEAR_LS" %}
+    external_function_param_casadi_free(&capsule->cost_y_e_fun);
+    external_function_param_casadi_free(&capsule->cost_y_e_fun_jac_ut_xt);
+    external_function_param_casadi_free(&capsule->cost_y_e_hess);
+{%- elif cost.cost_type_e == "EXTERNAL" %}
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun);
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac);
+    external_function_param_{{ cost.cost_ext_fun_type_e }}_free(&capsule->ext_cost_e_fun_jac_hess);
+{%- endif %}
+
+    // constraints
+{%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]);
+        external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]);
+    }
+  {%- if solver_options.hessian_approx == "EXACT" %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]);
+    }
+  {%- endif %}
+    free(capsule->nl_constr_h_fun_jac);
+    free(capsule->nl_constr_h_fun);
+  {%- if solver_options.hessian_approx == "EXACT" %}
+    free(capsule->nl_constr_h_fun_jac_hess);
+  {%- endif %}
+
+{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
+    for (int i = 0; i < {{ dims.N }}; i++)
+    {
+        external_function_param_casadi_free(&capsule->phi_constraint[i]);
+    }
+    free(capsule->phi_constraint);
+{%- endif %}
+
+{%- if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
+    external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac);
+    external_function_param_casadi_free(&capsule->nl_constr_h_e_fun);
+{%- if solver_options.hessian_approx == "EXACT" %}
+    external_function_param_casadi_free(&capsule->nl_constr_h_e_fun_jac_hess);
+{%- endif %}
+{%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
+    external_function_param_casadi_free(&capsule->phi_e_constraint);
+{%- endif %}
+
+    return 0;
+}
+
+ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule) { return capsule->nlp_in; }
+ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule) { return capsule->nlp_out; }
+ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule) { return capsule->nlp_solver; }
+ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule) { return capsule->nlp_config; }
+void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule) { return capsule->nlp_opts; }
+ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule) { return capsule->nlp_dims; }
+ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
+
+
+void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule)
+{
+    int sqp_iter, stat_m, stat_n, tmp_int;
+    ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
+    ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_n", &stat_n);
+    ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "stat_m", &stat_m);
+
+    {% set stat_n_max = 10 %}
+    double stat[{{ solver_options.nlp_solver_max_iter * stat_n_max }}];
+    ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "statistics", stat);
+
+    int nrow = sqp_iter+1 < stat_m ? sqp_iter+1 : stat_m;
+
+    printf("iter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter\n");
+    for (int i = 0; i < nrow; i++)
+    {
+        for (int j = 0; j < stat_n + 1; j++)
+        {
+            if (j == 0 || j > 4)
+            {
+                tmp_int = (int) stat[i + j * nrow];
+                printf("%d\t", tmp_int);
+            }
+            else
+            {
+                printf("%e\t", stat[i + j * nrow]);
+            }
+        }
+        printf("\n");
+    }
+}
+
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
new file mode 100644
index 000000000..e8966c254
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h
@@ -0,0 +1,133 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_SOLVER_{{ model.name }}_H_
+#define ACADOS_SOLVER_{{ model.name }}_H_
+
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// ** capsule for solver data **
+typedef struct nlp_solver_capsule
+{
+    // acados objects
+    ocp_nlp_in *nlp_in;
+    ocp_nlp_out *nlp_out;
+    ocp_nlp_solver *nlp_solver;
+    void *nlp_opts;
+    ocp_nlp_plan *nlp_solver_plan;
+    ocp_nlp_config *nlp_config;
+    ocp_nlp_dims *nlp_dims;
+
+    // number of expected runtime parameters
+    unsigned int nlp_np;
+
+    /* external functions */
+    // dynamics
+    external_function_param_casadi *forw_vde_casadi;
+    external_function_param_casadi *expl_ode_fun;
+    external_function_param_casadi *hess_vde_casadi;
+    external_function_param_casadi *impl_dae_fun;
+    external_function_param_casadi *impl_dae_fun_jac_x_xdot_z;
+    external_function_param_casadi *impl_dae_jac_x_xdot_u_z;
+    external_function_param_casadi *impl_dae_fun_jac_x_xdot_u;
+    external_function_param_casadi *impl_dae_hess;
+    external_function_param_casadi *gnsf_phi_fun;
+    external_function_param_casadi *gnsf_phi_fun_jac_y;
+    external_function_param_casadi *gnsf_phi_jac_y_uhat;
+    external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z;
+    external_function_param_casadi *gnsf_get_matrices_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun;
+    external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt;
+    external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess;
+
+    // cost
+    external_function_param_casadi *cost_y_fun;
+    external_function_param_casadi *cost_y_fun_jac_ut_xt;
+    external_function_param_casadi *cost_y_hess;
+    external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
+    external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
+    external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess;
+
+    external_function_param_casadi cost_y_0_fun;
+    external_function_param_casadi cost_y_0_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_0_hess;
+    external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
+    external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
+    external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess;
+
+    external_function_param_casadi cost_y_e_fun;
+    external_function_param_casadi cost_y_e_fun_jac_ut_xt;
+    external_function_param_casadi cost_y_e_hess;
+    external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
+    external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
+    external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess;
+
+    // constraints
+    external_function_param_casadi *phi_constraint;
+    external_function_param_casadi *nl_constr_h_fun_jac;
+    external_function_param_casadi *nl_constr_h_fun;
+    external_function_param_casadi *nl_constr_h_fun_jac_hess;
+
+    external_function_param_casadi phi_e_constraint;
+    external_function_param_casadi nl_constr_h_e_fun_jac;
+    external_function_param_casadi nl_constr_h_e_fun;
+    external_function_param_casadi nl_constr_h_e_fun_jac_hess;
+} nlp_solver_capsule;
+
+nlp_solver_capsule * {{ model.name }}_acados_create_capsule(void);
+int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule);
+
+int {{ model.name }}_acados_create(nlp_solver_capsule * capsule);
+int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *value, int np);
+int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule);
+int {{ model.name }}_acados_free(nlp_solver_capsule * capsule);
+void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule);
+
+ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule);
+ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule);
+ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule);
+ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule);
+void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule);
+ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule);
+ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SOLVER_{{ model.name }}_H_
diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
new file mode 100644
index 000000000..e70c5cafe
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
@@ -0,0 +1,761 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }}
+#define S_FUNCTION_LEVEL 2
+
+#define MDL_START
+
+// acados
+#include "acados/utils/print.h"
+#include "acados_c/sim_interface.h"
+#include "acados_c/external_function_interface.h"
+
+// example specific
+#include "{{ model.name }}_model/{{ model.name }}_model.h"
+#include "acados_solver_{{ model.name }}.h"
+
+#include "simstruc.h"
+
+{% if simulink_opts.samplingtime == "t0" -%}
+#define SAMPLINGTIME {{ solver_options.time_steps[0] }}
+{%- elif simulink_opts.samplingtime == "-1" -%}
+#define SAMPLINGTIME -1
+{%- else -%}
+  {{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }}
+{%- endif %}
+
+static void mdlInitializeSizes (SimStruct *S)
+{
+    // specify the number of continuous and discrete states
+    ssSetNumContStates(S, 0);
+    ssSetNumDiscStates(S, 0);
+
+  {%- for key, val in simulink_opts.inputs -%}
+    {%- if val != 0 and val != 1 -%}
+      {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
+    {%- endif -%}
+  {%- endfor -%}
+
+  {#- compute number of input ports #}
+  {%- set n_inputs = 0 -%}
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%}  {#- lbx_0 #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%}  {#- ubx_0 #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%}  {#- y_ref_0 -#}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%}  {#- y_ref -#}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%}  {#- y_ref_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%}  {#- ubx #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%}  {#- lbx_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%}  {#- lbu #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%}  {#- ubu #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+  {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+
+  {%- for key, val in simulink_opts.inputs -%}
+    {%- if val != 0 and val != 1 -%}
+      {{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
+    {%- endif -%}
+  {%- endfor -%}
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
+    {%- set n_inputs = n_inputs + 1 %}
+  {%- endif -%}
+  {%- if dims.ny > 0 and simulink_opts.inputs.cost_W -%}  {#- cost_W #}
+    {%- set n_inputs = n_inputs + 1 %}
+  {%- endif -%}
+  {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e -%}  {#- cost_W_e #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+
+  {%- if simulink_opts.inputs.x_init -%}  {#- x_init #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+
+  {%- if simulink_opts.inputs.u_init -%}  {#- u_init #}
+    {%- set n_inputs = n_inputs + 1 -%}
+  {%- endif -%}
+
+    // specify the number of input ports
+    if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
+        return;
+
+    // specify the number of output ports
+    {%- set_global n_outputs = 0 %}
+    {%- for key, val in simulink_opts.outputs %}
+      {%- if val == 1 %}
+        {%- set_global n_outputs = n_outputs + val %}
+      {%- elif val != 0 %}
+        {{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }}
+      {%- endif %}
+    {%- endfor %}
+    if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) )
+        return;
+
+    // specify dimension information for the input ports
+    {%- set i_input = -1 %}{# note here i_input is 0-based #}
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%}  {#- lbx_0 #}
+    {%- set i_input = i_input + 1 %}
+    // lbx_0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
+  {%- endif %}
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%}  {#- ubx_0 #}
+    {%- set i_input = i_input + 1 %}
+    // ubx_0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
+  {%- endif %}
+
+  {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
+    {%- set i_input = i_input + 1 %}
+    // parameters
+    ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }});
+  {%- endif %}
+
+  {%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %}
+    {%- set i_input = i_input + 1 %}
+    // y_ref_0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }});
+  {%- endif %}
+
+  {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
+    {%- set i_input = i_input + 1 %}
+    // y_ref
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }});
+  {%- endif %}
+
+  {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
+    {%- set i_input = i_input + 1 %}
+    // y_ref_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }});
+  {%- endif %}
+
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
+    {%- set i_input = i_input + 1 %}
+    // lbx
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
+  {%- endif %}
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%}  {#- ubx #}
+    {%- set i_input = i_input + 1 %}
+    // ubx
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
+  {%- endif %}
+
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%}  {#- lbx_e #}
+    {%- set i_input = i_input + 1 %}
+    // lbx_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
+  {%- endif %}
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
+    {%- set i_input = i_input + 1 %}
+    // ubx_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
+  {%- endif %}
+
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%}  {#- lbu #}
+    {%- set i_input = i_input + 1 %}
+    // lbu
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
+  {%- endif -%}
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%}  {#- ubu #}
+    {%- set i_input = i_input + 1 %}
+    // ubu
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
+  {%- endif -%}
+
+
+  {%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
+    {%- set i_input = i_input + 1 %}
+    // lg
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
+  {%- endif -%}
+  {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
+    {%- set i_input = i_input + 1 %}
+    // ug
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
+  {%- endif -%}
+
+  {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
+    {%- set i_input = i_input + 1 %}
+    // lh
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
+  {%- endif -%}
+  {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
+    {%- set i_input = i_input + 1 %}
+    // uh
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
+  {%- endif -%}
+
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
+    {%- set i_input = i_input + 1 %}
+    // cost_W_0
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }});
+  {%- endif %}
+
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W %}  {#- cost_W #}
+    {%- set i_input = i_input + 1 %}
+    // cost_W
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }});
+  {%- endif %}
+
+  {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %}  {#- cost_W_e #}
+    {%- set i_input = i_input + 1 %}
+    // cost_W_e
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }});
+  {%- endif %}
+
+  {%- if simulink_opts.inputs.x_init -%}  {#- x_init #}
+    {%- set i_input = i_input + 1 %}
+    // x_init
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }});
+  {%- endif -%}
+
+  {%- if simulink_opts.inputs.u_init -%}  {#- u_init #}
+    {%- set i_input = i_input + 1 %}
+    // u_init
+    ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }});
+  {%- endif -%}
+
+    /* specify dimension information for the OUTPUT ports */
+    {%- set i_output = -1 %}{# note here i_output is 0-based #}
+  {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} );
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.utraj == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} );
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.xtraj == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} );
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.solver_status == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.KKT_residual == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
+  {%- endif %}
+
+  {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.CPU_time == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.CPU_time_sim == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.CPU_time_qp == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.CPU_time_lin == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.sqp_iter == 1 %}
+    {%- set i_output = i_output + 1 %}
+    ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
+  {%- endif %}
+
+    // specify the direct feedthrough status
+    // should be set to 1 for all inputs used in mdlOutputs
+    {%- for i in range(end=n_inputs) %}
+    ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
+    {%- endfor %}
+
+    // one sample time
+    ssSetNumSampleTimes(S, 1);
+}
+
+
+#if defined(MATLAB_MEX_FILE)
+
+#define MDL_SET_INPUT_PORT_DIMENSION_INFO
+#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
+
+static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
+{
+    if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
+         return;
+}
+
+#endif /* MATLAB_MEX_FILE */
+
+
+static void mdlInitializeSampleTimes(SimStruct *S)
+{
+    ssSetSampleTime(S, 0, SAMPLINGTIME);
+    ssSetOffsetTime(S, 0, 0.0);
+}
+
+
+static void mdlStart(SimStruct *S)
+{
+    nlp_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
+    {{ model.name }}_acados_create(capsule);
+
+    ssSetUserData(S, (void*)capsule);
+}
+
+
+static void mdlOutputs(SimStruct *S, int_T tid)
+{
+    nlp_solver_capsule *capsule = ssGetUserData(S);
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
+    ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
+
+    InputRealPtrsType in_sign;
+
+    {%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%}
+
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}  {# y_ref_0 #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %}
+  {%- endif %}
+  {%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}  {# y_ref #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %}
+  {%- endif %}
+  {%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}  {# y_ref_e #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %}
+  {%- endif %}
+
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {# cost_W_0 #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %}
+  {%- endif %}
+  {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %}  {# cost_W #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %}
+  {%- endif %}
+  {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %}  {# cost_W_e #}
+    {%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %}
+  {%- endif %}
+
+    // local buffer
+    {%- set buffer_size = buffer_sizes | sort | last %}
+    real_t buffer[{{ buffer_size }}];
+
+    /* go through inputs */
+    {%- set i_input = -1 %}
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%}  {#- lbx_0 #}
+    // lbx_0
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.nbx_0 }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
+  {%- endif %}
+
+  {%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%}  {#- ubx_0 #}
+    // ubx_0
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.nbx_0 }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
+  {%- endif %}
+
+  {%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
+    // parameters - stage-variant !!!
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    // update value of parameters
+    for (int ii = 0; ii <= {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.np }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]);
+        {{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }});
+    }
+  {%- endif %}
+
+  {% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
+    // y_ref_0
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.ny_0 }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
+  {%- endif %}
+
+  {% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
+    // y_ref - for stages 1 to N-1
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.ny }}; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]);
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
+    }
+  {%- endif %}
+
+  {% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
+    // y_ref_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.ny_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer);
+  {%- endif %}
+
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
+    // lbx
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nbx }}; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
+    }
+  {%- endif %}
+  {%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%}  {#- ubx #}
+    // ubx
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 1; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nbx }}; jj++)
+            buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
+    }
+  {%- endif %}
+
+
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%}  {#- lbx_e #}
+    // lbx_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.nbx_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer);
+  {%- endif %}
+  {%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
+    // ubx_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.nbx_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer);
+  {%- endif %}
+
+
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%}  {#- lbu #}
+    // lbu
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nbu }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer);
+    }
+  {%- endif -%}
+  {%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%}  {#- ubu #}
+    // ubu
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nbu }}; jj++)
+            buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer);
+    }
+  {%- endif -%}
+
+  {%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
+    // lg
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.ng }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer);
+  {%- endif -%}
+  {%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
+    // ug
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.ng }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer);
+  {%- endif -%}
+  {%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
+    // lh
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.nh }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
+  {%- endif -%}
+  {%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
+    // uh
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+
+    for (int i = 0; i < {{ dims.nh }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
+  {%- endif -%}
+
+  {%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {# cost_W_0 #}
+    // cost_W_0
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer);
+  {%- endif %}
+
+  {%- if dims.ny > 0 and simulink_opts.inputs.cost_W %}  {# cost_W #}
+    // cost_W
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.ny * dims.ny }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    for (int ii = 1; ii < {{ dims.N }}; ii++)
+        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer);
+  {%- endif %}
+
+  {%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %}  {#- cost_W_e #}
+    // cost_W_e
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++)
+        buffer[i] = (double)(*in_sign[i]);
+
+    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer);
+  {%- endif %}
+
+  {%- if simulink_opts.inputs.x_init %}  {#- x_init #}
+    // x_init
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nx }}; jj++)
+            buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]);
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer);
+    }
+  {%- endif %}
+
+  {%- if simulink_opts.inputs.u_init %}  {#- u_init #}
+    // u_init
+    {%- set i_input = i_input + 1 %}
+    in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+    {
+        for (int jj = 0; jj < {{ dims.nu }}; jj++)
+            buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]);
+        ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer);
+    }
+  {%- endif %}
+
+    /* call solver */
+    int rti_phase = 0;
+    ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
+    int acados_status = {{ model.name }}_acados_solve(capsule);
+
+
+    /* set outputs */
+    // assign pointers to output signals
+    real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
+    int tmp_int;
+
+    {%- set i_output = -1 -%}{# note here i_output is 0-based #}
+  {%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.utraj == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }});
+    for (int ii = 0; ii < {{ dims.N }}; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
+                        "u", (void *) (out_utraj + ii * {{ dims.nu }}));
+  {%- endif %}
+
+  {% if simulink_opts.outputs.xtraj == 1 %}
+    {%- set i_output = i_output + 1 %}
+
+    out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }});
+    for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
+                        "x", (void *) (out_xtraj + ii * {{ dims.nx }}));
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.solver_status == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_status = ssGetOutputPortRealSignal(S, {{ i_output }});
+    *out_status = (real_t) acados_status;
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.KKT_residual == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }});
+    *out_KKT_res = (real_t) nlp_out->inf_norm_res;
+  {%- endif %}
+
+  {%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
+  {%- endif %}
+
+  {%- if simulink_opts.outputs.CPU_time == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }});
+    // get solution time
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
+  {%- endif -%}
+
+  {%- if simulink_opts.outputs.CPU_time_sim == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim);
+  {%- endif -%}
+
+  {%- if simulink_opts.outputs.CPU_time_qp == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp);
+  {%- endif -%}
+
+  {%- if simulink_opts.outputs.CPU_time_lin == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }});
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin);
+  {%- endif -%}
+
+  {%- if simulink_opts.outputs.sqp_iter == 1 %}
+    {%- set i_output = i_output + 1 %}
+    out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }});
+    // get sqp iter
+    ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
+    *out_sqp_iter = (real_t) tmp_int;
+  {%- endif %}
+
+}
+
+static void mdlTerminate(SimStruct *S)
+{
+    nlp_solver_capsule *capsule = ssGetUserData(S);
+
+    {{ model.name }}_acados_free(capsule);
+    {{ model.name }}_acados_free_capsule(capsule);
+}
+
+
+#ifdef  MATLAB_MEX_FILE
+#include "simulink.c"
+#else
+#include "cg_sfun.h"
+#endif
diff --git a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h
new file mode 100644
index 000000000..347446e3f
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_Y_0_COST
+#define {{ model.name }}_Y_0_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_type_0 == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_0_fun_n_in(void);
+int {{ model.name }}_cost_y_0_fun_n_out(void);
+
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_0_hess_n_in(void);
+int {{ model.name }}_cost_y_0_hess_n_out(void);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_Y_0_COST
diff --git a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h
new file mode 100644
index 000000000..acc99009f
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_Y_E_COST
+#define {{ model.name }}_Y_E_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_type_e == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_e_fun_n_in(void);
+int {{ model.name }}_cost_y_e_fun_n_out(void);
+
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_e_hess_n_in(void);
+int {{ model.name }}_cost_y_e_hess_n_out(void);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_Y_E_COST
diff --git a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h
new file mode 100644
index 000000000..1e03780cc
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_Y_COST
+#define {{ model.name }}_Y_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_type == "NONLINEAR_LS" %}
+int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_y_fun_sparsity_out(int);
+int {{ model.name }}_cost_y_fun_n_in(void);
+int {{ model.name }}_cost_y_fun_n_out(void);
+
+int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int);
+const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void);
+int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void);
+
+int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_y_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_y_hess_sparsity_out(int);
+int {{ model.name }}_cost_y_hess_n_in(void);
+int {{ model.name }}_cost_y_hess_n_out(void);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_Y_COST
diff --git a/pyextra/acados_template/c_templates_tera/external_cost.in.h b/pyextra/acados_template/c_templates_tera/external_cost.in.h
new file mode 100644
index 000000000..d200dba45
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/external_cost.in.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_EXT_COST
+#define {{ model.name }}_EXT_COST
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_ext_fun_type == "casadi" %}
+{% if cost.cost_type == "EXTERNAL" %}
+int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void);
+{% endif %}
+
+{% else %}
+int {{ cost.cost_function_ext_cost }}(void **, void **, void *);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_EXT_COST
diff --git a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h
new file mode 100644
index 000000000..8152447e5
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h
@@ -0,0 +1,75 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_EXT_COST_0
+#define {{ model.name }}_EXT_COST_0
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_ext_fun_type_0 == "casadi" %}
+
+{% if cost.cost_type_0 == "EXTERNAL" %}
+int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void);
+{% endif %}
+
+{% else %}
+int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_EXT_COST_0
diff --git a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h
new file mode 100644
index 000000000..56485db4c
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h
@@ -0,0 +1,74 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_EXT_COST_E
+#define {{ model.name }}_EXT_COST_E
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if cost.cost_ext_fun_type_e == "casadi" %}
+{% if cost.cost_type_e == "EXTERNAL" %}
+int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void);
+
+int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void);
+int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void);
+{% endif %}
+
+{% else %}
+int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_EXT_COST_E
diff --git a/pyextra/acados_template/c_templates_tera/h_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_constraint.in.h
new file mode 100644
index 000000000..e49176c6e
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/h_constraint.in.h
@@ -0,0 +1,70 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef {{ model.name }}_H_CONSTRAINT
+#define {{ model.name }}_H_CONSTRAINT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if dims.nh > 0 %}
+int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void);
+
+int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_n_in(void);
+int {{ model.name }}_constr_h_fun_n_out(void);
+
+{% if solver_options.hessian_approx == "EXACT" -%}
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int);
+const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void);
+int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void);
+{% endif %}
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_H_CONSTRAINT
diff --git a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h
new file mode 100644
index 000000000..a5dd71164
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef {{ model.name }}_H_E_CONSTRAINT
+#define {{ model.name }}_H_E_CONSTRAINT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if dims.nh_e > 0 %}
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int);
+const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void);
+
+int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int);
+const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int);
+int {{ model.name }}_constr_h_e_fun_n_in(void);
+int {{ model.name }}_constr_h_e_fun_n_out(void);
+
+{% if solver_options.hessian_approx == "EXACT" -%}
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int);
+const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void);
+int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void);
+{% endif %}
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_H_E_CONSTRAINT
diff --git a/pyextra/acados_template/c_templates_tera/main.in.c b/pyextra/acados_template/c_templates_tera/main.in.c
new file mode 100644
index 000000000..1cae69aea
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/main.in.c
@@ -0,0 +1,181 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_c/external_function_interface.h"
+#include "acados_solver_{{ model.name }}.h"
+
+
+int main()
+{
+
+    nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
+    int status = {{ model.name }}_acados_create(acados_ocp_capsule);
+
+    if (status)
+    {
+        printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
+    ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
+    ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
+    void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
+
+    // initial condition
+    int idxbx0[{{ dims.nbx_0 }}];
+    {%- for i in range(end=dims.nbx_0) %}
+    idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
+    {%- endfor %}
+
+    double lbx0[{{ dims.nbx_0 }}];
+    double ubx0[{{ dims.nbx_0 }}];
+    {%- for i in range(end=dims.nbx_0) %}
+    lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
+    ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+
+    // initialization for state values
+    double x_init[{{ dims.nx }}];
+    {%- for i in range(end=dims.nx) %}
+    x_init[{{ i }}] = 0.0;
+    {%- endfor %}
+
+    // initial value for control input
+    double u0[{{ dims.nu }}];
+    {%- for i in range(end=dims.nu) %}
+    u0[{{ i }}] = 0.0;
+    {%- endfor %}
+
+
+  {%- if dims.np > 0 %}
+    // set parameters
+    double p[{{ dims.np }}];
+    {% for item in parameter_values %}
+    p[{{ loop.index0 }}] = {{ item }};
+    {% endfor %}
+
+    for (int ii = 0; ii <= {{ dims.N }}; ii++)
+    {
+        {{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, {{ dims.np }});
+    }
+  {% endif %}{# if np > 0 #}
+
+    // prepare evaluation
+    int NTIMINGS = 1;
+    double min_time = 1e12;
+    double kkt_norm_inf;
+    double elapsed_time;
+    int sqp_iter;
+
+    double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
+    double utraj[{{ dims.nu }} * ({{ dims.N }})];
+
+
+    // solve ocp in loop
+    int rti_phase = 0;
+
+    for (int ii = 0; ii < NTIMINGS; ii++)
+    {
+        // initialize solution
+        for (int i = 0; i <= nlp_dims->N; i++)
+        {
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+        }
+        ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
+        status = {{ model.name }}_acados_solve(acados_ocp_capsule);
+        ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
+        min_time = MIN(elapsed_time, min_time);
+    }
+
+    /* print solution and statistics */
+    for (int ii = 0; ii <= nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
+    for (int ii = 0; ii < nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
+
+    printf("\n--- xtraj ---\n");
+    d_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
+    printf("\n--- utraj ---\n");
+    d_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
+    // ocp_nlp_out_print(nlp_solver->dims, nlp_out);
+
+    printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
+
+    if (status == ACADOS_SUCCESS)
+    {
+        printf("{{ model.name }}_acados_solve(): SUCCESS!\n");
+    }
+    else
+    {
+        printf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
+    }
+
+    // get solution
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
+    ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
+
+    {{ model.name }}_acados_print_stats(acados_ocp_capsule);
+
+    printf("\nSolver info:\n");
+    printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
+           sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
+
+    // free solver
+    status = {{ model.name }}_acados_free(acados_ocp_capsule);
+    if (status) {
+        printf("{{ model.name }}_acados_free() returned status %d. \n", status);
+    }
+    // free solver capsule
+    status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule);
+    if (status) {
+        printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status);
+    }
+
+    return status;
+}
diff --git a/pyextra/acados_template/c_templates_tera/main_mex.in.c b/pyextra/acados_template/c_templates_tera/main_mex.in.c
new file mode 100644
index 000000000..8da5db29a
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/main_mex.in.c
@@ -0,0 +1,184 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/ocp_nlp_interface.h"
+#include "acados_solver_{{ model.name }}.h"
+// mex
+#include "mex.h"
+
+/* auxilary mex */
+// prints a matrix in column-major format (exponential notation)
+void MEX_print_exp_mat(int m, int n, double *A, int lda)
+{
+	for (int i=0; i<m; i++)
+    {
+		for (int j=0; j<n; j++)
+        {
+			mexPrintf("%e\t", A[i+lda*j]);
+        }
+		mexPrintf("\n");
+    }
+	mexPrintf("\n");
+}
+
+// prints the transposed of a matrix in column-major format (exponential notation)
+void MEX_print_exp_tran_mat(int row, int col, double *A, int lda)
+{
+	for (int j=0; j<col; j++)
+    {
+		for (int i=0; i<row; i++)
+        {
+			mexPrintf("%e\t", A[i+lda*j]);
+        }
+		mexPrintf("\n");
+    }
+	mexPrintf("\n");
+}
+
+
+void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
+{
+
+    int status = 0;
+    status = {{ model.name }}_acados_create();
+
+    if (status)
+    {
+        mexPrintf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+
+    // get pointers to nlp solver related objects
+    ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config();
+    ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims();
+    ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in();
+    ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out();
+    ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver();
+    void *nlp_opts = {{ model.name }}_acados_get_nlp_opts();
+
+    // initial condition
+    int idxbx0[{{ dims.nbx_0 }}];
+    {% for i in range(end=dims.nbx_0) %}
+    idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
+    {%- endfor %}
+
+    double lbx0[{{ dims.nbx_0 }}];
+    double ubx0[{{ dims.nbx_0 }}];
+    {% for i in range(end=dims.nbx_0) %}
+    lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
+    ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
+    {%- endfor %}
+
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
+    ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
+
+    // initialization for state values
+    double x_init[{{ dims.nx }}];
+    {%- for i in range(end=dims.nx) %}
+    x_init[{{ i }}] = 0.0;
+    {%- endfor %}
+
+    // initial value for control input
+    double u0[{{ dims.nu }}];
+    {%- for i in range(end=dims.nu) %}
+    u0[{{ i }}] = 0.0;
+    {%- endfor %}
+
+    // prepare evaluation
+    int NTIMINGS = 10;
+    double min_time = 1e12;
+    double kkt_norm_inf;
+    double elapsed_time;
+    int sqp_iter;
+
+    double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
+    double utraj[{{ dims.nu }} * ({{ dims.N }})];
+
+    // solve ocp in loop
+    for (int ii = 0; ii < NTIMINGS; ii++)
+    {
+        // initialize primal solution
+        for (int i = 0; i <= nlp_dims->N; i++)
+        {
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
+            ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
+        }
+        status = {{ model.name }}_acados_solve();
+        ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
+        min_time = MIN(elapsed_time, min_time);
+    }
+
+    /* print solution and statistics */
+    for (int ii = 0; ii <= nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
+    for (int ii = 0; ii < nlp_dims->N; ii++)
+        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
+
+    mexPrintf("\n--- xtraj ---\n");
+    MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
+    mexPrintf("\n--- utraj ---\n");
+    MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
+
+    mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
+
+    if (status == ACADOS_SUCCESS)
+        mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n");
+    else
+        mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
+
+    // get solution
+    ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
+    ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
+
+    mexPrintf("\nSolver info:\n");
+    mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n",
+           sqp_iter, min_time*1000, kkt_norm_inf);
+
+    // free solver
+    status = {{ model.name }}_acados_free();
+    if (status)
+    {
+        mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
+    }
+
+    return;
+}
diff --git a/pyextra/acados_template/c_templates_tera/main_sim.in.c b/pyextra/acados_template/c_templates_tera/main_sim.in.c
new file mode 100644
index 000000000..b62bf8f7f
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/main_sim.in.c
@@ -0,0 +1,130 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// standard
+#include <stdio.h>
+#include <stdlib.h>
+// acados
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+#include "acados_c/sim_interface.h"
+#include "acados_sim_solver_{{ model.name }}.h"
+
+
+int main()
+{
+    int status = 0;
+    sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
+    status = {{ model.name }}_acados_sim_create(capsule);
+
+    if (status)
+    {
+        printf("acados_create() returned status %d. Exiting.\n", status);
+        exit(1);
+    }
+
+    sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
+    sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
+    sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
+    void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
+
+    // initial condition
+    double x_current[{{ dims.nx }}];
+    {%- for i in range(end=dims.nx) %}
+    x_current[{{ i }}] = 0.0;
+    {%- endfor %}
+
+  {% if constraints.lbx_0 %}
+    {%- for i in range(end=dims.nbx_0) %}
+    x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }};
+    {%- endfor %}
+    {% if dims.nbx_0 != dims.nx %}
+    printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0.");
+    {%- endif %}
+  {% else %}
+    printf("main_sim: initial state not defined, should be in lbx_0, using zero vector.");
+  {%- endif %}
+
+
+    // initial value for control input
+    double u0[{{ dims.nu }}];
+    {%- for i in range(end=dims.nu) %}
+    u0[{{ i }}] = 0.0;
+    {%- endfor %}
+
+  {%- if dims.np > 0 %}
+    // set parameters
+    double p[{{ dims.np }}];
+    {% for item in parameter_values %}
+    p[{{ loop.index0 }}] = {{ item }};
+    {% endfor %}
+
+    {{ model.name }}_acados_sim_update_params(capsule, p, {{ dims.np }});
+  {% endif %}{# if np > 0 #}
+
+    int n_sim_steps = 3;
+    // solve ocp in loop
+    for (int ii = 0; ii < n_sim_steps; ii++)
+    {
+        sim_in_set(acados_sim_config, acados_sim_dims,
+            acados_sim_in, "x", x_current);
+        status = {{ model.name }}_acados_sim_solve(capsule);
+
+        if (status != ACADOS_SUCCESS)
+        {
+            printf("acados_solve() failed with status %d.\n", status);
+        }
+
+        sim_out_get(acados_sim_config, acados_sim_dims,
+               acados_sim_out, "x", x_current);
+        
+        printf("\nx_current, %d\n", ii);
+        for (int jj = 0; jj < {{ dims.nx }}; jj++)
+        {
+            printf("%e\n", x_current[jj]);
+        }
+    }
+
+    printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
+
+    // free solver
+    status = {{ model.name }}_acados_sim_free(capsule);
+    if (status) {
+        printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status);
+    }
+
+    {{ model.name }}_acados_sim_solver_free_capsule(capsule);
+
+    return status;
+}
diff --git a/pyextra/acados_template/c_templates_tera/make_main_mex.in.m b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m
new file mode 100644
index 000000000..9188686a0
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/make_main_mex.in.m
@@ -0,0 +1,105 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+function make_main_mex_{{ model.name }}()
+
+    opts.output_dir = pwd;
+
+    % get acados folder
+    acados_folder = getenv('ACADOS_INSTALL_DIR');
+
+    % set paths
+    acados_include = ['-I' fullfile(acados_folder, 'include')];
+    template_lib_include = ['-l' 'acados_solver_{{ model.name }}'];
+    template_lib_path = ['-L' fullfile(pwd)];
+
+    acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
+    external_include = ['-I', fullfile(acados_folder, 'external')];
+    blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
+    hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
+
+    mex_names = { ...
+        'main_mex_{{ model.name }}' ...
+    };
+
+    mex_files = cell(length(mex_names), 1);
+    for k=1:length(mex_names)
+        mex_files{k} = fullfile([mex_names{k}, '.c']);
+    end
+
+    %% octave C flags
+    if is_octave()
+        if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
+            diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
+            diary on
+            mkoctfile -p CFLAGS
+            diary off
+            input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
+            cflags_tmp = fscanf(input_file, '%[^\n]s');
+            fclose(input_file);
+            if ~ismac()
+                cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
+            else
+                cflags_tmp = [cflags_tmp, ' -std=c99'];
+            end
+            input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
+            fprintf(input_file, '%s', cflags_tmp);
+            fclose(input_file);
+        end
+        % read cflags from file
+        input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
+        cflags_tmp = fscanf(input_file, '%[^\n]s');
+        fclose(input_file);
+        setenv('CFLAGS', cflags_tmp);
+    end
+
+    %% compile mex
+    for ii=1:length(mex_files)
+        disp(['compiling ', mex_files{ii}])
+        if is_octave()
+    %        mkoctfile -p CFLAGS
+            mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
+            acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+        else
+            if ismac()
+                FLAGS = 'CFLAGS=$CFLAGS -std=c99';
+            else
+                FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
+            end
+            mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
+                acados_lib_path, template_lib_path, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+        end
+    end
+
+
+end
\ No newline at end of file
diff --git a/pyextra/acados_template/c_templates_tera/make_mex.in.m b/pyextra/acados_template/c_templates_tera/make_mex.in.m
new file mode 100644
index 000000000..cde30f6f4
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/make_mex.in.m
@@ -0,0 +1,110 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+function make_mex_{{ model.name }}()
+
+    opts.output_dir = pwd;
+
+    % get acados folder
+    acados_folder = getenv('ACADOS_INSTALL_DIR');
+
+    % set paths
+    acados_include = ['-I' fullfile(acados_folder, 'include')];
+    template_lib_include = ['-l' 'acados_ocp_solver_{{ model.name }}'];
+    template_lib_path = ['-L' fullfile(pwd)];
+
+    acados_lib_path = ['-L' fullfile(acados_folder, 'lib')];
+    external_include = ['-I', fullfile(acados_folder, 'external')];
+    blasfeo_include = ['-I', fullfile(acados_folder, 'external', 'blasfeo', 'include')];
+    hpipm_include = ['-I', fullfile(acados_folder, 'external', 'hpipm', 'include')];
+
+    mex_include = ['-I', fullfile(acados_folder, 'interfaces', 'acados_matlab_octave')];
+
+    mex_names = { ...
+        'acados_mex_create_{{ model.name }}' ...
+        'acados_mex_free_{{ model.name }}' ...
+        'acados_mex_solve_{{ model.name }}' ...
+        'acados_mex_set_{{ model.name }}' ...
+    };
+
+    mex_files = cell(length(mex_names), 1);
+    for k=1:length(mex_names)
+        mex_files{k} = fullfile([mex_names{k}, '.c']);
+    end
+
+    %% octave C flags
+    if is_octave()
+        if ~exist(fullfile(opts.output_dir, 'cflags_octave.txt'), 'file')
+            diary(fullfile(opts.output_dir, 'cflags_octave.txt'));
+            diary on
+            mkoctfile -p CFLAGS
+            diary off
+            input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
+            cflags_tmp = fscanf(input_file, '%[^\n]s');
+            fclose(input_file);
+            if ~ismac()
+                cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp'];
+            else
+                cflags_tmp = [cflags_tmp, ' -std=c99'];
+            end
+            input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'w');
+            fprintf(input_file, '%s', cflags_tmp);
+            fclose(input_file);
+        end
+        % read cflags from file
+        input_file = fopen(fullfile(opts.output_dir, 'cflags_octave.txt'), 'r');
+        cflags_tmp = fscanf(input_file, '%[^\n]s');
+        fclose(input_file);
+        setenv('CFLAGS', cflags_tmp);
+    end
+
+    %% compile mex
+    for ii=1:length(mex_files)
+        disp(['compiling ', mex_files{ii}])
+        if is_octave()
+    %        mkoctfile -p CFLAGS
+            mex(acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
+            acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+        else
+            if ismac()
+                FLAGS = 'CFLAGS=$CFLAGS -std=c99';
+            else
+                FLAGS = 'CFLAGS=$CFLAGS -std=c99 -fopenmp';
+            end
+            mex(FLAGS, acados_include, template_lib_include, external_include, blasfeo_include, hpipm_include,...
+                acados_lib_path, template_lib_path, mex_include, '-lacados', '-lhpipm', '-lblasfeo', mex_files{ii})
+        end
+    end
+
+
+end
\ No newline at end of file
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
new file mode 100644
index 000000000..172da654e
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m
@@ -0,0 +1,335 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = { ...
+        {%- if solver_options.integrator_type == 'ERK' %}
+            '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c', ...
+            '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c',...
+            {%- if solver_options.hessian_approx == 'EXACT' %}
+            '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c',...
+            {%- endif %}
+        {%- elif solver_options.integrator_type == "IRK" %}
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c', ...
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c', ...
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c', ...
+            {%- if solver_options.hessian_approx == 'EXACT' %}
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c',...
+            {%- endif %}
+        {%- elif solver_options.integrator_type == "GNSF" %}
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c',...
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c',...
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c',...
+            '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c',...
+            '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c',...
+        {%- elif solver_options.integrator_type == "DISCRETE" %}
+            '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun.c',...
+            '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac.c',...
+        {%- if solver_options.hessian_approx == "EXACT" %}
+            '{{ model.name }}_model/{{ model.name }}_dyn_disc_phi_fun_jac_hess.c',...
+        {%- endif %}
+        {%- endif %}
+        {%- if cost.cost_type_0 == "NONLINEAR_LS" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_0_fun_jac_ut_xt.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_0_hess.c',...
+        {%- elif cost.cost_type_0 == "EXTERNAL" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_0_fun_jac_hess.c',...
+        {%- endif %}
+
+        {%- if cost.cost_type == "NONLINEAR_LS" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_fun_jac_ut_xt.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_hess.c',...
+        {%- elif cost.cost_type == "EXTERNAL" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_fun_jac_hess.c',...
+        {%- endif %}
+        {%- if cost.cost_type_e == "NONLINEAR_LS" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_e_fun_jac_ut_xt.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_y_e_hess.c',...
+        {%- elif cost.cost_type_e == "EXTERNAL" %}
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac.c',...
+            '{{ model.name }}_cost/{{ model.name }}_cost_ext_cost_e_fun_jac_hess.c',...
+        {%- endif %}
+        {%- if constraints.constr_type == "BGH"  and dims.nh > 0 %}
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun.c', ...
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt_hess.c', ...
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_fun_jac_uxt_zt.c', ...
+        {%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
+            '{{ model.name }}_constraints/{{ model.name }}_phi_constraint.c', ...
+        {%- endif %}
+        {%- if constraints.constr_type_e == "BGH"  and dims.nh_e > 0 %}
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun.c', ...
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess.c', ...
+            '{{ model.name }}_constraints/{{ model.name }}_constr_h_e_fun_jac_uxt_zt.c', ...
+        {%- elif constraints.constr_type_e == "BGP" and dims.nphi_e > 0 %}
+            '{{ model.name }}_constraints/{{ model.name }}_phi_e_constraint.c', ...
+        {%- endif %}
+            'acados_solver_sfunction_{{ model.name }}.c', ...
+            'acados_solver_{{ model.name }}.c'
+          };
+
+INC_PATH = '{{ acados_include_path }}';
+
+INCS = {['-I', fullfile(INC_PATH, 'blasfeo', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'hpipm', 'include')], ...
+        ['-I', fullfile(INC_PATH, 'acados')], ...
+        ['-I', fullfile(INC_PATH)]};
+
+{% if solver_options.qp_solver is containing("QPOASES") %}
+INCS{end+1} = ['-I', fullfile(INC_PATH, 'qpOASES_e')];
+{% endif %}
+
+CFLAGS = 'CFLAGS=$CFLAGS';
+LDFLAGS = 'LDFLAGS=$LDFLAGS';
+COMPFLAGS = 'COMPFLAGS=$COMPFLAGS';
+COMPDEFINES = 'COMPDEFINES=$COMPDEFINES';
+
+{% if solver_options.qp_solver is containing("QPOASES") %}
+CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPOASES ' ];
+COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPOASES ' ];
+{%- elif solver_options.qp_solver is containing("OSQP") %}
+CFLAGS = [ CFLAGS, ' -DACADOS_WITH_OSQP ' ];
+COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_OSQP ' ];
+{%- elif solver_options.qp_solver is containing("QPDUNES") %}
+CFLAGS = [ CFLAGS, ' -DACADOS_WITH_QPDUNES ' ];
+COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_QPDUNES ' ];
+{%- elif solver_options.qp_solver is containing("HPMPC") %}
+CFLAGS = [ CFLAGS, ' -DACADOS_WITH_HPMPC ' ];
+COMPDEFINES = [ COMPDEFINES, ' -DACADOS_WITH_HPMPC ' ];
+{% endif %}
+
+LIB_PATH = ['-L', fullfile('{{ acados_lib_path }}')];
+
+LIBS = {'-lacados', '-lhpipm', '-lblasfeo'};
+
+% acados linking libraries and flags
+{%- if acados_link_libs and os and os == "pc" %}
+LDFLAGS = [LDFLAGS ' {{ acados_link_libs.openmp }}'];
+COMPFLAGS = [COMPFLAGS ' {{ acados_link_libs.openmp }}'];
+LIBS{end+1} = '{{ acados_link_libs.qpoases }}';
+LIBS{end+1} = '{{ acados_link_libs.hpmpc }}';
+LIBS{end+1} = '{{ acados_link_libs.osqp }}';
+{%- else %}
+    {% if solver_options.qp_solver is containing("QPOASES") %}
+LIBS{end+1} = '-lqpOASES_e';
+    {% endif %}
+{%- endif %}
+
+mex('-v', '-O', CFLAGS, LDFLAGS, COMPFLAGS, COMPDEFINES, INCS{:}, ...
+    LIB_PATH, LIBS{:}, SOURCES{:}, ...
+    '-output', 'acados_solver_sfunction_{{ model.name }}' );
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_solver_sfunction_{{ model.name }}', '.', ...
+    eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n';
+i_in = 1;
+
+
+{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%}  {#- lbx_0 #}
+input_note = strcat(input_note, num2str(i_in), ') lbx_0 - lower bound on x for stage 0,',...
+                    ' size [{{ dims.nbx_0 }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%}  {#- ubx_0 #}
+input_note = strcat(input_note, num2str(i_in), ') ubx_0 - upper bound on x for stage 0,',...
+                    ' size [{{ dims.nbx_0 }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%}  {#- parameter_traj #}
+input_note = strcat(input_note, num2str(i_in), ') parameters - concatenated for all shooting nodes 0 to N+1,',...
+                    ' size [{{ (dims.N+1)*dims.np }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
+input_note = strcat(input_note, num2str(i_in), ') y_ref_0, size [{{ dims.ny_0 }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
+input_note = strcat(input_note, num2str(i_in), ') y_ref - concatenated for shooting nodes 1 to N-1,',...
+                    ' size [{{ (dims.N-1) * dims.ny }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
+input_note = strcat(input_note, num2str(i_in), ') y_ref_e, size [{{ dims.ny_e }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%}  {#- lbx #}
+input_note = strcat(input_note, num2str(i_in), ') lbx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%}  {#- ubx #}
+input_note = strcat(input_note, num2str(i_in), ') ubx for shooting nodes 1 to N-1, size [{{ (dims.N-1) * dims.nbx }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+
+{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%}  {#- lbx_e #}
+input_note = strcat(input_note, num2str(i_in), ') lbx_e (lbx at shooting node N), size [{{ dims.nbx_e }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%}  {#- ubx_e #}
+input_note = strcat(input_note, num2str(i_in), ') ubx_e (ubx at shooting node N), size [{{ dims.nbx_e }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%}  {#- lbu #}
+input_note = strcat(input_note, num2str(i_in), ') lbu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
+i_in = i_in + 1;
+{%- endif -%}
+{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%}  {#- ubu #}
+input_note = strcat(input_note, num2str(i_in), ') ubu for shooting nodes 0 to N-1, size [{{ dims.N*dims.nbu }}]\n ');
+i_in = i_in + 1;
+{%- endif -%}
+
+{%- if dims.ng > 0 and simulink_opts.inputs.lg -%}  {#- lg #}
+input_note = strcat(input_note, num2str(i_in), ') lg, size [{{ dims.ng }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+{%- if dims.ng > 0 and simulink_opts.inputs.ug -%}  {#- ug #}
+input_note = strcat(input_note, num2str(i_in), ') ug, size [{{ dims.ng }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.nh > 0 and simulink_opts.inputs.lh -%}  {#- lh #}
+input_note = strcat(input_note, num2str(i_in), ') lh, size [{{ dims.nh }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+{%- if dims.nh > 0 and simulink_opts.inputs.uh -%}  {#- uh #}
+input_note = strcat(input_note, num2str(i_in), ') uh, size [{{ dims.nh }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %}  {#- cost_W_0 #}
+input_note = strcat(input_note, num2str(i_in), ') cost_W_0 in column-major format, size [{{ dims.ny_0 * dims.ny_0 }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %}  {#- cost_W #}
+input_note = strcat(input_note, num2str(i_in), ') cost_W in column-major format, that is set for all intermediate shooting nodes: 1 to N-1, size [{{ dims.ny * dims.ny }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %}  {#- cost_W_e #}
+input_note = strcat(input_note, num2str(i_in), ') cost_W_e in column-major format, size [{{ dims.ny_e * dims.ny_e }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if simulink_opts.inputs.x_init %}  {#- x_init #}
+input_note = strcat(input_note, num2str(i_in), ') initialization of x for all shooting nodes, size [{{ dims.nx * (dims.N+1) }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if simulink_opts.inputs.u_init %}  {#- u_init #}
+input_note = strcat(input_note, num2str(i_in), ') initialization of u for shooting nodes 0 to N-1, size [{{ dims.nu * (dims.N) }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = 'Outputs are:\n';
+i_out = 0;
+
+{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') u0, control input at node 0, size [{{ dims.nu }}]\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.utraj == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') utraj, control input concatenated for nodes 0 to N-1, size [{{ dims.nu * dims.N }}]\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.xtraj == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') xtraj, state concatenated for nodes 0 to N, size [{{ dims.nx * (dims.N + 1) }}]\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.solver_status == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') acados solver status (0 = SUCCESS)\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.KKT_residual == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') KKT residual\n ');
+{%- endif %}
+
+{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') x1, state at node 1\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.CPU_time == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n ');
+{%- endif %}
+
+{%- if simulink_opts.outputs.sqp_iter == 1 %}
+i_out = i_out + 1;
+output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n ');
+{%- endif %}
+
+fprintf(output_note)
diff --git a/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
new file mode 100644
index 000000000..1c5cf0b12
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/make_sfun_sim.in.m
@@ -0,0 +1,99 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+SOURCES = [ 'acados_sim_solver_sfunction_{{ model.name }}.c ', ...
+            'acados_sim_solver_{{ model.name }}.c ', ...
+            {%- if  solver_options.integrator_type == 'ERK' %}
+            '{{ model.name }}_model/{{ model.name }}_expl_ode_fun.c ', ...
+            '{{ model.name }}_model/{{ model.name }}_expl_vde_forw.c ',...
+                {%- if solver_options.hessian_approx == 'EXACT' %}
+            '{{ model.name }}_model/{{ model.name }}_expl_ode_hess.c ',...
+                {%- endif %}
+        {%- elif solver_options.integrator_type == "IRK" %}
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_fun.c ', ...
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_z.c ', ...
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.c ', ...
+                {%- if solver_options.hessian_approx == 'EXACT' %}
+            '{{ model.name }}_model/{{ model.name }}_impl_dae_hess.c ',...
+                {%- endif %}
+            {%- elif solver_options.integrator_type == "GNSF" %}
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.c '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.c '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_phi_jac_y_uhat.c '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz.c '
+            '{{ model.name }}_model/{{ model.name }}_gnsf_get_matrices_fun.c '
+            {%- endif %}
+          ];
+
+INC_PATH = '{{ acados_include_path }}';
+
+INCS = [ ' -I', fullfile(INC_PATH, 'blasfeo', 'include'), ...
+         ' -I', fullfile(INC_PATH, 'hpipm', 'include'), ...
+        ' -I', INC_PATH, ' -I', fullfile(INC_PATH, 'acados'), ' '];
+
+CFLAGS  = ' -O';
+
+LIB_PATH = '{{ acados_lib_path }}';
+
+LIBS = '-lacados -lblasfeo -lhpipm';
+
+eval( [ 'mex -v -output  acados_sim_solver_sfunction_{{ model.name }} ', ...
+    CFLAGS, INCS, ' ', SOURCES, ' -L', LIB_PATH, ' ', LIBS ]);
+
+fprintf( [ '\n\nSuccessfully created sfunction:\nacados_sim_solver_sfunction_{{ model.name }}', '.', ...
+    eval('mexext')] );
+
+
+%% print note on usage of s-function
+fprintf('\n\nNote: Usage of Sfunction is as follows:\n')
+input_note = 'Inputs are:\n1) x0, initial state, size [{{ dims.nx }}]\n ';
+i_in = 2;
+{%- if dims.nu > 0 %}
+input_note = strcat(input_note, num2str(i_in), ') u, size [{{ dims.nu }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+{%- if dims.np > 0 %}
+input_note = strcat(input_note, num2str(i_in), ') parameters, size [{{ dims.np }}]\n ');
+i_in = i_in + 1;
+{%- endif %}
+
+
+fprintf(input_note)
+
+disp(' ')
+
+output_note = strcat('Outputs are:\n', ...
+                '1) x1 - simulated state, size [{{ dims.nx }}]\n');
+
+fprintf(output_note)
diff --git a/pyextra/acados_template/c_templates_tera/mex_solver.in.m b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
new file mode 100644
index 000000000..728741a46
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/mex_solver.in.m
@@ -0,0 +1,166 @@
+%
+% Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+% Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+% Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+% Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+%
+
+classdef {{ model.name }}_mex_solver < handle
+
+    properties
+        C_ocp
+        C_ocp_ext_fun
+        cost_ext_fun_type
+        cost_ext_fun_type_e
+    end % properties
+
+
+
+    methods
+
+        % constructor
+        function obj = {{ model.name }}_mex_solver()
+            make_mex_{{ model.name }}();
+            [obj.C_ocp, obj.C_ocp_ext_fun] = acados_mex_create_{{ model.name }}();
+            % to have path to destructor when changing directory
+            addpath('.')
+            obj.cost_ext_fun_type = '{{ cost.cost_ext_fun_type }}';
+            obj.cost_ext_fun_type_e = '{{ cost.cost_ext_fun_type_e }}';
+        end
+
+        % destructor
+        function delete(obj)
+            if ~isempty(obj.C_ocp)
+                acados_mex_free_{{ model.name }}(obj.C_ocp);
+            end
+        end
+
+        % solve
+        function solve(obj)
+            acados_mex_solve_{{ model.name }}(obj.C_ocp);
+        end
+
+        % set -- borrowed from MEX interface
+        function set(varargin)
+            obj = varargin{1};
+            field = varargin{2};
+            value = varargin{3};
+            if ~isa(field, 'char')
+                error('field must be a char vector, use '' ''');
+            end
+            if nargin==3
+                acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value);
+            elseif nargin==4
+                stage = varargin{4};
+                acados_mex_set_{{ model.name }}(obj.cost_ext_fun_type, obj.cost_ext_fun_type_e, obj.C_ocp, obj.C_ocp_ext_fun, field, value, stage);
+            else
+                disp('acados_ocp.set: wrong number of input arguments (2 or 3 allowed)');
+            end
+        end
+
+        function value = get_cost(obj)
+            value = ocp_get_cost(obj.C_ocp);
+        end
+
+        % get -- borrowed from MEX interface
+        function value = get(varargin)
+            % usage:
+            % obj.get(field, value, [stage])
+            obj = varargin{1};
+            field = varargin{2};
+            if any(strfind('sens', field))
+                error('field sens* (sensitivities of optimal solution) not yet supported for templated MEX.')
+            end
+            if ~isa(field, 'char')
+                error('field must be a char vector, use '' ''');
+            end
+
+            if nargin==2
+                value = ocp_get(obj.C_ocp, field);
+            elseif nargin==3
+                stage = varargin{3};
+                value = ocp_get(obj.C_ocp, field, stage);
+            else
+                disp('acados_ocp.get: wrong number of input arguments (1 or 2 allowed)');
+            end
+        end
+
+
+        % print
+        function print(varargin)
+            if nargin < 2
+                field = 'stat';
+            else
+                field = varargin{2};
+            end
+
+            obj = varargin{1};
+
+            if strcmp(field, 'stat')
+                stat = obj.get('stat');
+                {%- if solver_options.nlp_solver_type == "SQP" %}
+                fprintf('\niter\tres_stat\tres_eq\t\tres_ineq\tres_comp\tqp_stat\tqp_iter');
+                if size(stat,2)>7
+                    fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
+                end
+                fprintf('\n');
+                for jj=1:size(stat,1)
+                    fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(jj,1), stat(jj,2), stat(jj,3), stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
+                    if size(stat,2)>7
+                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,8), stat(jj,9), stat(jj,10), stat(jj,11));
+                    end
+                    fprintf('\n');
+                end
+                fprintf('\n');
+                {%- else %}
+                fprintf('\niter\tqp_status\tqp_iter');
+                if size(stat,2)>3
+                    fprintf('\tqp_res_stat\tqp_res_eq\tqp_res_ineq\tqp_res_comp');
+                end
+                fprintf('\n');
+                for jj=1:size(stat,1)
+                    fprintf('%d\t%d\t\t%d', stat(jj,1), stat(jj,2), stat(jj,3));
+                    if size(stat,2)>3
+                        fprintf('\t%e\t%e\t%e\t%e', stat(jj,4), stat(jj,5), stat(jj,6), stat(jj,7));
+                    end
+                    fprintf('\n');
+                end
+                {% endif %}
+
+            else
+                fprintf('unsupported field in function print of acados_ocp.print, got %s', field);
+                keyboard
+            end
+
+        end
+
+    end % methods
+
+end % class
+
diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h
new file mode 100644
index 000000000..661811232
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/model.in.h
@@ -0,0 +1,209 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef {{ model.name }}_MODEL
+#define {{ model.name }}_MODEL
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{%- if solver_options.hessian_approx %}
+	{%- set hessian_approx = solver_options.hessian_approx %}
+{%- elif solver_options.sens_hess %}
+	{%- set hessian_approx = "EXACT" %}
+{%- else %}
+	{%- set hessian_approx = "GAUSS_NEWTON" %}
+{%- endif %}
+
+{% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %}
+// implicit ODE
+int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_impl_dae_fun_sparsity_in(int);
+const int *{{ model.name }}_impl_dae_fun_sparsity_out(int);
+int {{ model.name }}_impl_dae_fun_n_in(void);
+int {{ model.name }}_impl_dae_fun_n_out(void);
+
+// implicit ODE
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *);
+const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int);
+const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void);
+
+// implicit ODE
+int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *);
+const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int);
+const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int);
+int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void);
+int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void);
+
+// implicit ODE - for lifted_irk
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *);
+const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int);
+const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void);
+int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void);
+
+{%- if hessian_approx == "EXACT" %}
+int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_impl_dae_hess_sparsity_in(int);
+const int *{{ model.name }}_impl_dae_hess_sparsity_out(int);
+int {{ model.name }}_impl_dae_hess_n_in(void);
+int {{ model.name }}_impl_dae_hess_n_out(void);
+{%- endif %}
+
+{% elif solver_options.integrator_type == "GNSF" %}
+/* GNSF Functions */
+// used to import model matrices
+int        {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int);
+const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_in(void);
+int        {{ model.name }}_gnsf_get_matrices_fun_n_out(void);
+
+// phi_fun
+int        {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int);
+const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int);
+int        {{ model.name }}_gnsf_phi_fun_n_in(void);
+int        {{ model.name }}_gnsf_phi_fun_n_out(void);
+
+// phi_fun_jac_y
+int        {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int);
+const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int);
+int        {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void);
+int        {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void);
+
+// phi_jac_y_uhat
+int        {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int);
+const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int);
+int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void);
+int        {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void);
+
+// f_lo_fun_jac_x1k1uz
+int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem);
+int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *);
+const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int);
+const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int);
+int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void);
+int        {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void);
+
+{% elif solver_options.integrator_type == "ERK" %}
+/* explicit ODE */
+
+// explicit ODE
+int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_expl_ode_fun_sparsity_in(int);
+const int *{{ model.name }}_expl_ode_fun_sparsity_out(int);
+int {{ model.name }}_expl_ode_fun_n_in(void);
+int {{ model.name }}_expl_ode_fun_n_out(void);
+
+// explicit forward VDE
+int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *);
+const int *{{ model.name }}_expl_vde_forw_sparsity_in(int);
+const int *{{ model.name }}_expl_vde_forw_sparsity_out(int);
+int {{ model.name }}_expl_vde_forw_n_in(void);
+int {{ model.name }}_expl_vde_forw_n_out(void);
+
+// explicit adjoint VDE
+int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *);
+const int *{{ model.name }}_expl_vde_adj_sparsity_in(int);
+const int *{{ model.name }}_expl_vde_adj_sparsity_out(int);
+int {{ model.name }}_expl_vde_adj_n_in(void);
+int {{ model.name }}_expl_vde_adj_n_out(void);
+
+{%- if hessian_approx == "EXACT" %}
+int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_expl_ode_hess_sparsity_in(int);
+const int *{{ model.name }}_expl_ode_hess_sparsity_out(int);
+int {{ model.name }}_expl_ode_hess_n_in(void);
+int {{ model.name }}_expl_ode_hess_n_out(void);
+{%- endif %}
+
+{% elif solver_options.integrator_type == "DISCRETE" %}
+
+{% if model.dyn_ext_fun_type == "casadi" %}
+int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *);
+const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int);
+const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int);
+int {{ model.name }}_dyn_disc_phi_fun_n_in(void);
+int {{ model.name }}_dyn_disc_phi_fun_n_out(void);
+
+int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *);
+const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int);
+const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int);
+int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void);
+int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void);
+
+{%- if hessian_approx == "EXACT" %}
+int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *);
+const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int);
+const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int);
+int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void);
+int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void);
+{%- endif %}
+{% else %}
+  {%- if hessian_approx == "EXACT" %}
+int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *);
+  {% endif %}
+int {{ model.dyn_disc_fun_jac }}(void **, void **, void *);
+int {{ model.dyn_disc_fun }}(void **, void **, void *);
+{% endif %}
+
+
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_MODEL
diff --git a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h
new file mode 100644
index 000000000..283ed7f88
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h
@@ -0,0 +1,55 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef {{ model.name }}_PHI_CONSTRAINT
+#define {{ model.name }}_PHI_CONSTRAINT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if dims.nphi > 0 %}
+// implicit ODE
+int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *);
+const int *{{ model.name }}_phi_constraint_sparsity_in(int);
+const int *{{ model.name }}_phi_constraint_sparsity_out(int);
+int {{ model.name }}_phi_constraint_n_in(void);
+int {{ model.name }}_phi_constraint_n_out(void);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_PHI_CONSTRAINT
diff --git a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h
new file mode 100644
index 000000000..dc8e293ad
--- /dev/null
+++ b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h
@@ -0,0 +1,21 @@
+#ifndef {{ model.name }}_PHI_E_CONSTRAINT
+#define {{ model.name }}_PHI_E_CONSTRAINT
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+{% if dims.nphi_e > 0 %}
+int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem);
+int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *);
+const int *{{ model.name }}_phi_e_constraint_sparsity_in(int);
+const int *{{ model.name }}_phi_e_constraint_sparsity_out(int);
+int {{ model.name }}_phi_e_constraint_n_in(void);
+int {{ model.name }}_phi_e_constraint_n_out(void);
+{% endif %}
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // {{ model.name }}_PHI_E_CONSTRAINT
diff --git a/pyextra/acados_template/generate_c_code_constraint.py b/pyextra/acados_template/generate_c_code_constraint.py
new file mode 100644
index 000000000..93e919c56
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_constraint.py
@@ -0,0 +1,180 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
+
+def generate_c_code_constraint( model, con_name, is_terminal, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+    # load constraint variables and expression
+    x = model.x
+    p = model.p
+
+    if isinstance(x, casadi.MX):
+        symbol = MX.sym
+    else:
+        symbol = SX.sym
+
+    if is_terminal:
+        con_h_expr = model.con_h_expr_e
+        con_phi_expr = model.con_phi_expr_e
+        # create dummy u, z
+        u = symbol('u', 0, 0)
+        z = symbol('z', 0, 0)
+    else:
+        con_h_expr = model.con_h_expr
+        con_phi_expr = model.con_phi_expr
+        u = model.u
+        z = model.z
+
+    if (not is_empty(con_h_expr)) and (not is_empty(con_phi_expr)):
+        raise Exception("acados: you can either have constraint_h, or constraint_phi, not both.")
+
+    if not (is_empty(con_h_expr) and is_empty(con_phi_expr)):
+        if is_empty(con_h_expr):
+            constr_type = 'BGP'
+        else:
+            constr_type = 'BGH'
+
+        if is_empty(p):
+            p = symbol('p', 0, 0)
+
+        if is_empty(z):
+            z = symbol('z', 0, 0)
+
+        if not (is_empty(con_h_expr)) and opts['generate_hess']:
+            # multipliers for hessian
+            nh = casadi_length(con_h_expr)
+            lam_h = symbol('lam_h', nh, 1)
+
+        # set up & change directory
+        code_export_dir = opts["code_export_directory"]
+        if not os.path.exists(code_export_dir):
+            os.makedirs(code_export_dir)
+
+        cwd = os.getcwd()
+        os.chdir(code_export_dir)
+        gen_dir = con_name + '_constraints'
+        if not os.path.exists(gen_dir):
+            os.mkdir(gen_dir)
+        gen_dir_location = './' + gen_dir
+        os.chdir(gen_dir_location)
+
+        # export casadi functions
+        if constr_type == 'BGH':
+            if is_terminal:
+                fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt'
+            else:
+                fun_name = con_name + '_constr_h_fun_jac_uxt_zt'
+
+            jac_ux_t = transpose(jacobian(con_h_expr, vertcat(u,x)))
+            jac_z_t = jacobian(con_h_expr, z)
+            constraint_fun_jac_tran = Function(fun_name, [x, u, z, p], \
+                    [con_h_expr, jac_ux_t, jac_z_t])
+
+            constraint_fun_jac_tran.generate(fun_name, casadi_opts)
+            if opts['generate_hess']:
+
+                if is_terminal:
+                    fun_name = con_name + '_constr_h_e_fun_jac_uxt_zt_hess'
+                else:
+                    fun_name = con_name + '_constr_h_fun_jac_uxt_zt_hess'
+
+                # adjoint
+                adj_ux = jtimes(con_h_expr, vertcat(u, x), lam_h, True)
+                # hessian
+                hess_ux = jacobian(adj_ux, vertcat(u, x))
+
+                adj_z = jtimes(con_h_expr, z, lam_h, True)
+                hess_z = jacobian(adj_z, z)
+
+                # set up functions
+                constraint_fun_jac_tran_hess = \
+                    Function(fun_name, [x, u, lam_h, z, p], \
+                      [con_h_expr, jac_ux_t, hess_ux, jac_z_t, hess_z])
+
+                # generate C code
+                constraint_fun_jac_tran_hess.generate(fun_name, casadi_opts)
+
+            if is_terminal:
+                fun_name = con_name + '_constr_h_e_fun'
+            else:
+                fun_name = con_name + '_constr_h_fun'
+            h_fun = Function(fun_name, [x, u, z, p], [con_h_expr])
+            h_fun.generate(fun_name, casadi_opts)
+
+        else: # BGP constraint
+            if is_terminal:
+                fun_name = con_name + '_phi_e_constraint'
+                r = model.con_r_in_phi_e
+                con_r_expr = model.con_r_expr_e
+            else:
+                fun_name = con_name + '_phi_constraint'
+                r = model.con_r_in_phi
+                con_r_expr = model.con_r_expr
+
+            nphi = casadi_length(con_phi_expr)
+            con_phi_expr_x_u_z = substitute(con_phi_expr, r, con_r_expr)
+            phi_jac_u = jacobian(con_phi_expr_x_u_z, u)
+            phi_jac_x = jacobian(con_phi_expr_x_u_z, x)
+            phi_jac_z = jacobian(con_phi_expr_x_u_z, z)
+
+            hess = hessian(con_phi_expr[0], r)[0]
+            for i in range(1, nphi):
+                hess = vertcat(hess, hessian(con_phi_expr[i], r)[0])
+
+            r_jac_u = jacobian(con_r_expr, u)
+            r_jac_x = jacobian(con_r_expr, x)
+
+            constraint_phi = \
+                Function(fun_name, [x, u, z, p], \
+                    [con_phi_expr_x_u_z, \
+                    vertcat(transpose(phi_jac_u), \
+                    transpose(phi_jac_x)), \
+                    transpose(phi_jac_z), \
+                    hess, vertcat(transpose(r_jac_u), \
+                    transpose(r_jac_x))])
+
+            constraint_phi.generate(fun_name, casadi_opts)
+
+        # change directory back
+        os.chdir(cwd)
+
+    return
diff --git a/pyextra/acados_template/generate_c_code_discrete_dynamics.py b/pyextra/acados_template/generate_c_code_discrete_dynamics.py
new file mode 100644
index 000000000..531b5ed9d
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_discrete_dynamics.py
@@ -0,0 +1,99 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
+
+def generate_c_code_discrete_dynamics( model, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+    
+    # load model
+    x = model.x
+    u = model.u
+    p = model.p
+    phi = model.disc_dyn_expr
+    model_name = model.name
+    nx = x.size()[0]
+
+
+    if isinstance(phi, casadi.MX):
+        symbol = MX.sym
+    elif isinstance(phi, casadi.SX):
+        symbol = SX.sym
+    else:
+        Exception("generate_c_code_disc_dyn: disc_dyn_expr must be a CasADi expression, you have type: {}".format(type(phi)))
+
+    # assume nx1 = nx !!!
+    lam = symbol('lam', nx, 1)
+
+    # generate jacobians
+    ux = vertcat(u,x)
+    jac_ux = jacobian(phi, ux)
+    # generate adjoint
+    adj_ux = jtimes(phi, ux, lam, True)
+    # generate hessian
+    hess_ux = jacobian(adj_ux, ux)
+    
+    ## change directory
+    code_export_dir = opts["code_export_directory"]
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    model_dir = model_name + '_model'
+    if not os.path.exists(model_dir):
+        os.mkdir(model_dir)
+    model_dir_location = './' + model_dir
+    os.chdir(model_dir_location)
+
+    # set up & generate Functions
+    fun_name = model_name + '_dyn_disc_phi_fun'
+    phi_fun = Function(fun_name, [x, u, p], [phi])
+    phi_fun.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_dyn_disc_phi_fun_jac'
+    phi_fun_jac_ut_xt = Function(fun_name, [x, u, p], [phi, jac_ux.T])
+    phi_fun_jac_ut_xt.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_dyn_disc_phi_fun_jac_hess'
+    phi_fun_jac_ut_xt_hess = Function(fun_name, [x, u, lam, p], [phi, jac_ux.T, hess_ux])
+    phi_fun_jac_ut_xt_hess.generate(fun_name, casadi_opts)
+    
+    os.chdir(cwd)
diff --git a/pyextra/acados_template/generate_c_code_explicit_ode.py b/pyextra/acados_template/generate_c_code_explicit_ode.py
new file mode 100644
index 000000000..fb0c020b0
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_explicit_ode.py
@@ -0,0 +1,124 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
+
+def generate_c_code_explicit_ode( model, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+
+    generate_hess = opts["generate_hess"]
+    code_export_dir = opts["code_export_directory"]
+
+    # load model
+    x = model.x
+    u = model.u
+    p = model.p
+    f_expl = model.f_expl_expr
+    model_name = model.name
+
+    ## get model dimensions
+    nx = x.size()[0]
+    nu = u.size()[0]
+
+    if isinstance(f_expl, casadi.MX):
+        symbol = MX.sym
+    elif isinstance(f_expl, casadi.SX):
+        symbol = SX.sym
+    else:
+        raise Exception("Invalid type for f_expl! Possible types are 'SX' and 'MX'. Exiting.")
+    ## set up functions to be exported
+    Sx = symbol('Sx', nx, nx)
+    Sp = symbol('Sp', nx, nu)
+    lambdaX = symbol('lambdaX', nx, 1)
+
+    fun_name = model_name + '_expl_ode_fun'
+
+    ## Set up functions
+    expl_ode_fun = Function(fun_name, [x, u, p], [f_expl])
+
+    vdeX = jtimes(f_expl,x,Sx)
+    vdeP = jacobian(f_expl,u) + jtimes(f_expl,x,Sp)
+
+    fun_name = model_name + '_expl_vde_forw'
+
+    expl_vde_forw = Function(fun_name, [x, Sx, Sp, u, p], [f_expl, vdeX, vdeP])
+
+    adj = jtimes(f_expl, vertcat(x, u), lambdaX, True)
+
+    fun_name = model_name + '_expl_vde_adj'
+    expl_vde_adj = Function(fun_name, [x, lambdaX, u, p], [adj])
+
+    if generate_hess:
+        S_forw = vertcat(horzcat(Sx, Sp), horzcat(DM.zeros(nu,nx), DM.eye(nu)))
+        hess = mtimes(transpose(S_forw),jtimes(adj, vertcat(x,u), S_forw))
+        hess2 = []
+        for j in range(nx+nu):
+            for i in range(j,nx+nu):
+                hess2 = vertcat(hess2, hess[i,j])
+
+        fun_name = model_name + '_expl_ode_hess'
+        expl_ode_hess = Function(fun_name, [x, Sx, Sp, lambdaX, u, p], [adj, hess2])
+
+    ## generate C code
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    model_dir = model_name + '_model'
+    if not os.path.exists(model_dir):
+        os.mkdir(model_dir)
+    model_dir_location = './' + model_dir
+    os.chdir(model_dir_location)
+    fun_name = model_name + '_expl_ode_fun'
+    expl_ode_fun.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_expl_vde_forw'
+    expl_vde_forw.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_expl_vde_adj'
+    expl_vde_adj.generate(fun_name, casadi_opts)
+
+    if generate_hess:
+        fun_name = model_name + '_expl_ode_hess'
+        expl_ode_hess.generate(fun_name, casadi_opts)
+    os.chdir(cwd)
+
+    return
diff --git a/pyextra/acados_template/generate_c_code_external_cost.py b/pyextra/acados_template/generate_c_code_external_cost.py
new file mode 100644
index 000000000..bc21f85f7
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_external_cost.py
@@ -0,0 +1,110 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import SX, MX, Function, transpose, vertcat, horzcat, hessian, CasadiMeta
+from .utils import ALLOWED_CASADI_VERSIONS, casadi_version_warning
+
+
+def generate_c_code_external_cost(model, stage_type, opts):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
+
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+    x = model.x
+    p = model.p
+
+    if isinstance(x, MX):
+        symbol = MX.sym
+    else:
+        symbol = SX.sym
+
+    if stage_type == 'terminal':
+        suffix_name = "_cost_ext_cost_e_fun"
+        suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_e_fun_jac"
+        u = symbol("u", 0, 0)
+        ext_cost = model.cost_expr_ext_cost_e
+
+    elif stage_type == 'path':
+        suffix_name = "_cost_ext_cost_fun"
+        suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_fun_jac"
+        u = model.u
+        ext_cost = model.cost_expr_ext_cost
+
+    elif stage_type == 'initial':
+        suffix_name = "_cost_ext_cost_0_fun"
+        suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
+        suffix_name_jac = "_cost_ext_cost_0_fun_jac"
+        u = model.u
+        ext_cost = model.cost_expr_ext_cost_0
+
+    # set up functions to be exported
+    fun_name = model.name + suffix_name
+    fun_name_hess = model.name + suffix_name_hess
+    fun_name_jac = model.name + suffix_name_jac
+
+    # generate expression for full gradient and Hessian
+    full_hess, grad = hessian(ext_cost, vertcat(u, x))
+
+    ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
+    ext_cost_fun_jac_hess = Function(
+        fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]
+    )
+    ext_cost_fun_jac = Function(
+        fun_name_jac, [x, u, p], [ext_cost, grad]
+    )
+
+    # generate C code
+    code_export_dir = opts["code_export_directory"]
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    gen_dir = model.name + '_cost'
+    if not os.path.exists(gen_dir):
+        os.mkdir(gen_dir)
+    gen_dir_location = "./" + gen_dir
+    os.chdir(gen_dir_location)
+
+    ext_cost_fun.generate(fun_name, casadi_opts)
+    ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
+    ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
+
+    os.chdir(cwd)
+    return
diff --git a/pyextra/acados_template/generate_c_code_gnsf.py b/pyextra/acados_template/generate_c_code_gnsf.py
new file mode 100644
index 000000000..3203cbbcc
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_gnsf.py
@@ -0,0 +1,131 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_version_warning
+
+def generate_c_code_gnsf( model, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+    model_name = model.name
+    code_export_dir = opts["code_export_directory"]
+
+    # set up directory
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    model_dir = model_name + '_model'
+    if not os.path.exists(model_dir):
+        os.mkdir(model_dir)
+    model_dir_location = './' + model_dir
+    os.chdir(model_dir_location)
+
+    # obtain gnsf dimensions
+    get_matrices_fun = model.get_matrices_fun
+    phi_fun = model.phi_fun
+
+    size_gnsf_A = get_matrices_fun.size_out(0)
+    gnsf_nx1 = size_gnsf_A[1]
+    gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    gnsf_nuhat = max(phi_fun.size_in(1))
+    gnsf_ny = max(phi_fun.size_in(0))
+    gnsf_nout = max(phi_fun.size_out(0))
+
+    # set up expressions
+    # if the model uses MX because of cost/constraints
+    # the DAE can be exported as SX -> detect GNSF in Matlab
+    # -> evaluated SX GNSF functions with MX.
+    u = model.u
+
+    if isinstance(u, casadi.MX):
+        symbol = MX.sym
+    else:
+        symbol = SX.sym
+
+    y = symbol("y", gnsf_ny, 1)
+    uhat = symbol("uhat", gnsf_nuhat, 1)
+    p = model.p
+    x1 = symbol("gnsf_x1", gnsf_nx1, 1)
+    x1dot = symbol("gnsf_x1dot", gnsf_nx1, 1)
+    z1 = symbol("gnsf_z1", gnsf_nz1, 1)
+    dummy = symbol("gnsf_dummy", 1, 1)
+    empty_var = symbol("gnsf_empty_var", 0, 0)
+
+    ## generate C code
+    fun_name = model_name + '_gnsf_phi_fun'
+    phi_fun_ = Function(fun_name, [y, uhat, p], [phi_fun(y, uhat, p)])
+    phi_fun_.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_gnsf_phi_fun_jac_y'
+    phi_fun_jac_y = model.phi_fun_jac_y
+    phi_fun_jac_y_ = Function(fun_name, [y, uhat, p], phi_fun_jac_y(y, uhat, p))
+    phi_fun_jac_y_.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_gnsf_phi_jac_y_uhat'
+    phi_jac_y_uhat = model.phi_jac_y_uhat
+    phi_jac_y_uhat_ = Function(fun_name, [y, uhat, p], phi_jac_y_uhat(y, uhat, p))
+    phi_jac_y_uhat_.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_gnsf_f_lo_fun_jac_x1k1uz'
+    f_lo_fun_jac_x1k1uz = model.f_lo_fun_jac_x1k1uz
+    f_lo_fun_jac_x1k1uz_eval = f_lo_fun_jac_x1k1uz(x1, x1dot, z1, u, p)
+
+    # avoid codegeneration issue
+    if not isinstance(f_lo_fun_jac_x1k1uz_eval, tuple) and is_empty(f_lo_fun_jac_x1k1uz_eval):
+        f_lo_fun_jac_x1k1uz_eval = [empty_var]
+
+    f_lo_fun_jac_x1k1uz_ = Function(fun_name, [x1, x1dot, z1, u, p],
+                 f_lo_fun_jac_x1k1uz_eval)
+    f_lo_fun_jac_x1k1uz_.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_gnsf_get_matrices_fun'
+    get_matrices_fun_ = Function(fun_name, [dummy], get_matrices_fun(1))
+    get_matrices_fun_.generate(fun_name, casadi_opts)
+
+    # remove fields for json dump
+    del model.phi_fun
+    del model.phi_fun_jac_y
+    del model.phi_jac_y_uhat
+    del model.f_lo_fun_jac_x1k1uz
+    del model.get_matrices_fun
+
+    os.chdir(cwd)
+
+    return
diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py
new file mode 100644
index 000000000..e0138c69f
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_implicit_ode.py
@@ -0,0 +1,139 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, is_empty, casadi_length, casadi_version_warning
+
+def generate_c_code_implicit_ode( model, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+    generate_hess = opts["generate_hess"]
+    code_export_dir = opts["code_export_directory"]
+
+    ## load model
+    x = model.x
+    xdot = model.xdot
+    u = model.u
+    z = model.z
+    p = model.p
+    f_impl = model.f_impl_expr
+    model_name = model.name
+
+    ## get model dimensions
+    nx = casadi_length(x)
+    nu = casadi_length(u)
+    nz = casadi_length(z)
+
+    ## generate jacobians
+    jac_x       = jacobian(f_impl, x)
+    jac_xdot    = jacobian(f_impl, xdot)
+    jac_u       = jacobian(f_impl, u)
+    jac_z       = jacobian(f_impl, z)
+
+    ## generate hessian
+    x_xdot_z_u = vertcat(x, xdot, z, u)
+
+    if isinstance(x, casadi.MX):
+        symbol = MX.sym
+    else:
+        symbol = SX.sym
+
+    multiplier  = symbol('multiplier', nx + nz)
+
+    ADJ = jtimes(f_impl, x_xdot_z_u, multiplier, True)
+    HESS = jacobian(ADJ, x_xdot_z_u)
+
+    ## Set up functions
+    p = model.p
+    fun_name = model_name + '_impl_dae_fun'
+    impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
+    impl_dae_fun_jac_x_xdot_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
+
+    # fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
+    # impl_dae_fun_jac_x_xdot = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_z])
+
+    # fun_name = model_name + '_impl_dae_jac_x_xdot_u'
+    # impl_dae_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
+    impl_dae_fun_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u, jac_z])
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
+    impl_dae_fun_jac_x_xdot_u = Function(fun_name, [x, xdot, u, z, p], [f_impl, jac_x, jac_xdot, jac_u])
+
+    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
+    impl_dae_jac_x_xdot_u_z = Function(fun_name, [x, xdot, u, z, p], [jac_x, jac_xdot, jac_u, jac_z])
+
+
+    fun_name = model_name + '_impl_dae_hess'
+    impl_dae_hess = Function(fun_name, [x, xdot, u, z, multiplier, p], [HESS])
+
+    # generate C code
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    model_dir = model_name + '_model'
+    if not os.path.exists(model_dir):
+        os.mkdir(model_dir)
+    model_dir_location = './' + model_dir
+    os.chdir(model_dir_location)
+
+    fun_name = model_name + '_impl_dae_fun'
+    impl_dae_fun.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_z'
+    impl_dae_fun_jac_x_xdot_z.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_impl_dae_jac_x_xdot_u_z'
+    impl_dae_jac_x_xdot_u_z.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z'
+    impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts)
+
+    fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u'
+    impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts)
+
+    if generate_hess:
+        fun_name = model_name + '_impl_dae_hess'
+        impl_dae_hess.generate(fun_name, casadi_opts)
+
+    os.chdir(cwd)
diff --git a/pyextra/acados_template/generate_c_code_nls_cost.py b/pyextra/acados_template/generate_c_code_nls_cost.py
new file mode 100644
index 000000000..cdb0bf001
--- /dev/null
+++ b/pyextra/acados_template/generate_c_code_nls_cost.py
@@ -0,0 +1,113 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os
+from casadi import *
+from .utils import ALLOWED_CASADI_VERSIONS, casadi_length, casadi_version_warning
+
+def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
+
+    casadi_version = CasadiMeta.version()
+    casadi_opts = dict(mex=False, casadi_int='int', casadi_real='double')
+
+    if casadi_version not in (ALLOWED_CASADI_VERSIONS):
+        casadi_version_warning(casadi_version)
+
+    x = model.x
+    p = model.p
+
+    if isinstance(x, casadi.MX):
+        symbol = MX.sym
+    else:
+        symbol = SX.sym
+
+    if stage_type == 'terminal':
+        middle_name = '_cost_y_e'
+        u = symbol('u', 0, 0)
+        cost_expr = model.cost_y_expr_e
+
+    elif stage_type == 'initial':
+        middle_name = '_cost_y_0'
+        u = model.u
+        cost_expr = model.cost_y_expr_0
+
+    elif stage_type == 'path':
+        middle_name = '_cost_y'
+        u = model.u
+        cost_expr = model.cost_y_expr
+
+    # set up directory
+    code_export_dir = opts["code_export_directory"]
+    if not os.path.exists(code_export_dir):
+        os.makedirs(code_export_dir)
+
+    cwd = os.getcwd()
+    os.chdir(code_export_dir)
+    gen_dir = cost_name + '_cost'
+    if not os.path.exists(gen_dir):
+        os.mkdir(gen_dir)
+    gen_dir_location = './' + gen_dir
+    os.chdir(gen_dir_location)
+
+    # set up expressions
+    cost_jac_expr = transpose(jacobian(cost_expr, vertcat(u, x)))
+
+    ny = casadi_length(cost_expr)
+
+    y = symbol('y', ny, 1)
+
+    y_adj = jtimes(cost_expr, vertcat(u, x), y, True)
+    y_hess = jacobian(y_adj, vertcat(u, x))
+
+    ## generate C code
+    suffix_name = '_fun'
+    fun_name = cost_name + middle_name + suffix_name
+    y_fun = Function( fun_name, [x, u, p], \
+            [ cost_expr ])
+    y_fun.generate( fun_name, casadi_opts )
+
+    suffix_name = '_fun_jac_ut_xt'
+    fun_name = cost_name + middle_name + suffix_name
+    y_fun_jac_ut_xt = Function(fun_name, [x, u, p], \
+            [ cost_expr, cost_jac_expr ])
+    y_fun_jac_ut_xt.generate( fun_name, casadi_opts )
+
+    suffix_name = '_hess'
+    fun_name = cost_name + middle_name + suffix_name
+    y_hess = Function(fun_name, [x, u, y, p], [ y_hess ])
+    y_hess.generate( fun_name, casadi_opts )
+
+    os.chdir(cwd)
+
+    return
+
diff --git a/pyextra/acados_template/simulink_default_opts.json b/pyextra/acados_template/simulink_default_opts.json
new file mode 100644
index 000000000..258a224cb
--- /dev/null
+++ b/pyextra/acados_template/simulink_default_opts.json
@@ -0,0 +1,39 @@
+{
+    "outputs": {
+        "u0": 1,
+        "utraj": 0,
+        "xtraj": 0,
+        "solver_status": 1,
+        "KKT_residual": 1,
+        "x1": 1,
+        "CPU_time": 1,
+        "CPU_time_sim": 0,
+        "CPU_time_qp": 0,
+        "CPU_time_lin": 0,
+        "sqp_iter": 1
+    },
+    "inputs": {
+        "lbx_0": 1,
+        "ubx_0": 1,
+        "parameter_traj": 1,
+        "y_ref_0": 1,
+        "y_ref": 1,
+        "y_ref_e": 1,
+        "lbx": 1,
+        "ubx": 1,
+        "lbx_e": 1,
+        "ubx_e": 1,
+        "lbu": 1,
+        "ubu": 1,
+        "lg": 1,
+        "ug": 1,
+        "lh": 1,
+        "uh": 1,
+        "cost_W_0": 0,
+        "cost_W": 0,
+        "cost_W_e": 0,
+        "x_init": 0,
+        "u_init": 0
+    },
+    "samplingtime": "t0"
+}
diff --git a/pyextra/acados_template/utils.py b/pyextra/acados_template/utils.py
new file mode 100644
index 000000000..3f002b98c
--- /dev/null
+++ b/pyextra/acados_template/utils.py
@@ -0,0 +1,440 @@
+#
+# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import os, sys, json
+import urllib.request
+import shutil
+import numpy as np
+from casadi import SX, MX, DM, Function, CasadiMeta
+
+ALLOWED_CASADI_VERSIONS = ('3.5.5', '3.5.4', '3.5.3', '3.5.2', '3.5.1', '3.4.5', '3.4.0')
+
+TERA_VERSION = "0.0.34"
+
+def get_acados_path():
+    ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
+    if not ACADOS_PATH:
+        acados_template_path = os.path.dirname(os.path.abspath(__file__))
+        acados_path = os.path.join(acados_template_path, '../../../')
+        ACADOS_PATH = os.path.realpath(acados_path)
+        msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, '
+        msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH)
+        msg += 'Please export ACADOS_SOURCE_DIR to not avoid this warning.'
+        print(msg)
+    return ACADOS_PATH
+
+
+def get_tera_exec_path():
+    TERA_PATH = os.environ.get('TERA_PATH')
+    if not TERA_PATH:
+        TERA_PATH = os.path.join(get_acados_path(), 'bin/t_renderer')
+    return TERA_PATH
+
+
+platform2tera = {
+    "linux": "linux",
+    "darwin": "osx",
+    "win32": "window.exe"
+}
+
+
+def casadi_version_warning(casadi_version):
+    msg =  'Warning: Please note that the following versions of CasADi  are '
+    msg += 'officially supported: {}.\n '.format(" or ".join(ALLOWED_CASADI_VERSIONS))
+    msg += 'If there is an incompatibility with the CasADi generated code, '
+    msg += 'please consider changing your CasADi version.\n'
+    msg += 'Version {} currently in use.'.format(casadi_version)
+    print(msg)
+
+
+def is_column(x):
+    if isinstance(x, np.ndarray):
+        if x.ndim == 1:
+            return True
+        elif x.ndim == 2 and x.shape[1] == 1:
+            return True
+        else:
+            return False
+    elif isinstance(x, (MX, SX, DM)):
+        if x.shape[1] == 1:
+            return True
+        elif x.shape[0] == 0 and x.shape[1] == 0:
+            return True
+        else:
+            return False
+    elif x == None or x == []:
+        return False
+    else:
+        raise Exception("is_column expects one of the following types: np.ndarray, casadi.MX, casadi.SX."
+                        + " Got: " + str(type(x)))
+
+
+def is_empty(x):
+    if isinstance(x, (MX, SX, DM)):
+        return x.is_empty()
+    elif isinstance(x, np.ndarray):
+        if np.prod(x.shape) == 0:
+            return True
+        else:
+            return False
+    elif x == None or x == []:
+        return True
+    else:
+        raise Exception("is_empty expects one of the following types: casadi.MX, casadi.SX, "
+                        + "None, numpy array empty list. Got: " + str(type(x)))
+
+
+def casadi_length(x):
+    if isinstance(x, (MX, SX, DM)):
+        return int(np.prod(x.shape))
+    else:
+        raise Exception("casadi_length expects one of the following types: casadi.MX, casadi.SX."
+                        + " Got: " + str(type(x)))
+
+
+def make_model_consistent(model):
+    x = model.x
+    xdot = model.xdot
+    u = model.u
+    z = model.z
+    p = model.p
+
+    if isinstance(x, MX):
+        symbol = MX.sym
+    elif isinstance(x, SX):
+        symbol = SX.sym
+    else:
+        raise Exception("model.x must be casadi.SX or casadi.MX, got {}".format(type(x)))
+
+    if is_empty(p):
+        model.p = symbol('p', 0, 0)
+
+    if is_empty(z):
+        model.z = symbol('z', 0, 0)
+
+    return model
+
+
+def get_tera():
+    tera_path = get_tera_exec_path()
+    acados_path = get_acados_path()
+
+    if os.path.exists(tera_path) and os.access(tera_path, os.X_OK):
+        return tera_path
+
+    repo_url = "https://github.com/acados/tera_renderer/releases"
+    url = "{}/download/v{}/t_renderer-v{}-{}".format(
+        repo_url, TERA_VERSION, TERA_VERSION, platform2tera[sys.platform])
+
+    manual_install = 'For manual installation follow these instructions:\n'
+    manual_install += '1 Download binaries from {}\n'.format(url)
+    manual_install += '2 Copy them in {}/bin\n'.format(acados_path)
+    manual_install += '3 Strip the version and platform from the binaries: '
+    manual_install += 'as t_renderer-v0.0.34-X -> t_renderer)\n'
+    manual_install += '4 Enable execution privilege on the file "t_renderer" with:\n'
+    manual_install += '"chmod +x {}"\n\n'.format(tera_path)
+
+    msg = "\n"
+    msg += 'Tera template render executable not found, '
+    msg += 'while looking in path:\n{}\n'.format(tera_path)
+    msg += 'In order to be able to render the templates, '
+    msg += 'you need to download the tera renderer binaries from:\n'
+    msg += '{}\n\n'.format(repo_url)
+    msg += 'Do you wish to set up Tera renderer automatically?\n'
+    msg += 'y/N? (press y to download tera or any key for manual installation)\n'
+
+    if input(msg) == 'y':
+        print("Dowloading {}".format(url))
+        with urllib.request.urlopen(url) as response, open(tera_path, 'wb') as out_file:
+            shutil.copyfileobj(response, out_file)
+        print("Successfully downloaded t_renderer.")
+        os.chmod(tera_path, 0o755)
+        return tera_path
+
+    msg_cancel = "\nYou cancelled automatic download.\n\n"
+    msg_cancel += manual_install
+    msg_cancel += "Once installed re-run your script.\n\n"
+    print(msg_cancel)
+
+    sys.exit(1)
+
+
+def render_template(in_file, out_file, template_dir, json_path):
+    cwd = os.getcwd()
+    if not os.path.exists(template_dir):
+        os.mkdir(template_dir)
+    os.chdir(template_dir)
+
+    tera_path = get_tera()
+
+    # setting up loader and environment
+    acados_path = os.path.dirname(os.path.abspath(__file__))
+
+    template_glob = acados_path + '/c_templates_tera/*'
+    acados_template_path = acados_path + '/c_templates_tera'
+
+    # call tera as system cmd
+    os_cmd = "{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'".format(
+        tera_path=tera_path,
+        template_glob=template_glob,
+        json_path=json_path,
+        in_file=in_file,
+        out_file=out_file
+    )
+    status = os.system(os_cmd)
+    if (status != 0):
+        raise Exception('Rendering of {} failed! Exiting.\n'.format(in_file))
+
+    os.chdir(cwd)
+
+
+## Conversion functions
+def np_array_to_list(np_array):
+    if isinstance(np_array, (np.ndarray)):
+        return np_array.tolist()
+    elif isinstance(np_array, (SX)):
+        return DM(np_array).full()
+    elif isinstance(np_array, (DM)):
+        return np_array.full()
+    else:
+        raise(Exception(
+            "Cannot convert to list type {}".format(type(np_array))
+        ))
+
+
+def format_class_dict(d):
+    """
+    removes the __ artifact from class to dict conversion
+    """
+    out = {}
+    for k, v in d.items():
+        if isinstance(v, dict):
+            v = format_class_dict(v)
+
+        out_key = k.split('__', 1)[-1]
+        out[k.replace(k, out_key)] = v
+    return out
+
+
+def acados_class2dict(class_instance):
+    """
+    removes the __ artifact from class to dict conversion
+    """
+
+    d = dict(class_instance.__dict__)
+    out = {}
+    for k, v in d.items():
+        if isinstance(v, dict):
+            v = format_class_dict(v)
+
+        out_key = k.split('__', 1)[-1]
+        out[k.replace(k, out_key)] = v
+    return out
+
+
+def ocp_check_against_layout(ocp_nlp, ocp_dims):
+    """
+    Check dimensions against layout
+    Parameters
+    ---------
+    ocp_nlp : dict
+        dictionary loaded from JSON to be post-processed.
+
+    ocp_dims : instance of AcadosOcpDims
+    """
+
+    # load JSON layout
+    current_module = sys.modules[__name__]
+    acados_path = os.path.dirname(current_module.__file__)
+    with open(acados_path + '/acados_layout.json', 'r') as f:
+        ocp_nlp_layout = json.load(f)
+
+    ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
+    return
+
+
+def ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, layout):
+
+    for key, item in ocp_nlp.items():
+
+        try:
+            layout_of_key = layout[key]
+        except KeyError:
+            raise Exception("ocp_check_against_layout_recursion: field" \
+                            " '{0}' is not in layout but in OCP description.".format(key))
+
+        if isinstance(item, dict):
+            ocp_check_against_layout_recursion(item, ocp_dims, layout_of_key)
+
+        if 'ndarray' in layout_of_key:
+            if isinstance(item, int) or isinstance(item, float):
+                item = np.array([item])
+        if isinstance(item, (list, np.ndarray)) and (layout_of_key[0] != 'str'):
+            dim_layout = []
+            dim_names = layout_of_key[1]
+
+            for dim_name in dim_names:
+                dim_layout.append(ocp_dims[dim_name])
+
+            dims = tuple(dim_layout)
+
+            item = np.array(item)
+            item_dims = item.shape
+            if len(item_dims) != len(dims):
+                raise Exception('Mismatching dimensions for field {0}. ' \
+                    'Expected {1} dimensional array, got {2} dimensional array.' \
+                        .format(key, len(dims), len(item_dims)))
+
+            if np.prod(item_dims) != 0 or np.prod(dims) != 0:
+                if dims != item_dims:
+                    raise Exception('acados -- mismatching dimensions for field {0}. ' \
+                        'Provided data has dimensions {1}, ' \
+                        'while associated dimensions {2} are {3}' \
+                            .format(key, item_dims, dim_names, dims))
+    return
+
+
+def J_to_idx(J):
+    nrows = J.shape[0]
+    idx = np.zeros((nrows, ))
+    for i in range(nrows):
+        this_idx = np.nonzero(J[i,:])[0]
+        if len(this_idx) != 1:
+            raise Exception('Invalid J matrix structure detected, ' \
+                'must contain one nonzero element per row. Exiting.')
+        if this_idx.size > 0 and J[i,this_idx[0]] != 1:
+            raise Exception('J matrices can only contain 1s. Exiting.')
+        idx[i] = this_idx[0]
+    return idx
+
+
+def J_to_idx_slack(J):
+    nrows = J.shape[0]
+    ncol = J.shape[1]
+    idx = np.zeros((ncol, ))
+    i_idx = 0
+    for i in range(nrows):
+        this_idx = np.nonzero(J[i,:])[0]
+        if len(this_idx) == 1:
+            idx[i_idx] = i
+            i_idx = i_idx + 1
+        elif len(this_idx) > 1:
+            raise Exception('J_to_idx_slack: Invalid J matrix. Exiting. ' \
+                'Found more than one nonzero in row ' + str(i))
+        if this_idx.size > 0 and J[i,this_idx[0]] != 1:
+            raise Exception('J_to_idx_slack: J matrices can only contain 1s, ' \
+                 'got J(' + str(i) + ', ' + str(this_idx[0]) + ') = ' + str(J[i,this_idx[0]]) )
+    if not i_idx == ncol:
+            raise Exception('J_to_idx_slack: J must contain a 1 in every column!')
+    return idx
+
+
+def acados_dae_model_json_dump(model):
+
+    # load model
+    x = model.x
+    xdot = model.xdot
+    u = model.u
+    z = model.z
+    p = model.p
+
+    f_impl = model.f_impl_expr
+    model_name = model.name
+
+    # create struct with impl_dae_fun, casadi_version
+    fun_name = model_name + '_impl_dae_fun'
+    impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
+
+    casadi_version = CasadiMeta.version()
+    str_impl_dae_fun = impl_dae_fun.serialize()
+
+    dae_dict = {"str_impl_dae_fun": str_impl_dae_fun, "casadi_version": casadi_version}
+
+    # dump
+    json_file = model_name + '_acados_dae.json'
+    with open(json_file, 'w') as f:
+        json.dump(dae_dict, f, default=np_array_to_list, indent=4, sort_keys=True)
+    print("dumped ", model_name, " dae to file:", json_file, "\n")
+
+
+def set_up_imported_gnsf_model(acados_formulation):
+
+    gnsf = acados_formulation.gnsf_model
+
+    # check CasADi version
+    # dump_casadi_version = gnsf['casadi_version']
+    # casadi_version = CasadiMeta.version()
+
+    # if not casadi_version == dump_casadi_version:
+    #     print("WARNING: GNSF model was dumped with another CasADi version.\n"
+    #             + "This might yield errors. Please use the same version for compatibility, serialize version: "
+    #             + dump_casadi_version + " current Python CasADi verison: " + casadi_version)
+    #     input("Press any key to attempt to continue...")
+
+    # load model
+    phi_fun = Function.deserialize(gnsf['phi_fun'])
+    phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
+    phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
+    get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
+
+    # obtain gnsf dimensions
+    size_gnsf_A = get_matrices_fun.size_out(0)
+    acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1]
+    acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
+    acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1))
+    acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0))
+    acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0))
+
+    # save gnsf functions in model
+    acados_formulation.model.phi_fun = phi_fun
+    acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y
+    acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
+    acados_formulation.model.get_matrices_fun = get_matrices_fun
+
+    if "f_lo_fun_jac_x1k1uz" in gnsf:
+        f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
+        acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
+    else:
+        dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1)
+        dummy_var_x1dot = SX.sym('dummy_var_x1dot', acados_formulation.dims.gnsf_nx1)
+        dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1)
+        dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu)
+        dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np)
+        empty_var = SX.sym('empty_var', 0, 0)
+
+        empty_fun = Function('empty_fun', \
+            [dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
+                [empty_var])
+        acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun
+
+    del acados_formulation.gnsf_model
diff --git a/rednose/SConscript b/rednose/SConscript
index 36daf2cd7..ca1b28423 100644
--- a/rednose/SConscript
+++ b/rednose/SConscript
@@ -1,4 +1,4 @@
-Import('env', 'envCython', 'arch', 'rednose_config')
+Import('env', 'envCython', 'arch', 'rednose_config', 'common')
 
 generated_folder = rednose_config['generated_folder']
 
@@ -34,6 +34,7 @@ libkf = env.SharedLibrary(f'{generated_folder}/libkf', lib_target)
 
 lenv = envCython.Clone()
 lenv["LINKFLAGS"] += [libkf[0].get_labspath()]
+lenv["LIBS"] = ['zmq', common] + lenv["LIBS"]
 ekf_sym_so = lenv.Program('#rednose/helpers/ekf_sym_pyx.so', [ekf_sym_pyx, ekf_sym_cc, common_ekf])
 lenv.Depends(ekf_sym_so, libkf)
 
diff --git a/rednose/helpers/ekf_sym.cc b/rednose/helpers/ekf_sym.cc
index 365a6f989..be3f51609 100644
--- a/rednose/helpers/ekf_sym.cc
+++ b/rednose/helpers/ekf_sym.cc
@@ -1,4 +1,5 @@
 #include "ekf_sym.h"
+#include "logger/logger.h"
 
 using namespace EKFS;
 using namespace Eigen;
@@ -88,7 +89,7 @@ std::optional<Estimate> EKFSym::predict_and_update_batch(double t, int kind, std
   std::deque<Observation> rewound;
   if (!std::isnan(this->filter_time) && t < this->filter_time) {
     if (this->rewind_t.empty() || t < this->rewind_t.front() || t < this->rewind_t.back() - this->max_rewind_age) {
-      std::cout << "observation too old at " << t << " with filter at " << this->filter_time << ", ignoring" << std::endl;
+      LOGD("observation too old at %d with filter at %d, ignoring!", t, this->filter_time);
       return std::nullopt;
     }
     rewound = this->rewind(t);
diff --git a/rednose/logger/logger.h b/rednose/logger/logger.h
new file mode 100644
index 000000000..e21509352
--- /dev/null
+++ b/rednose/logger/logger.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#ifdef SWAGLOG
+#include "selfdrive/common/swaglog.h"
+#else
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif
diff --git a/release/build_release3.sh b/release/build_release.sh
similarity index 65%
rename from release/build_release3.sh
rename to release/build_release.sh
index 37cea5204..95fcfea1a 100755
--- a/release/build_release3.sh
+++ b/release/build_release.sh
@@ -9,7 +9,17 @@ cd $DIR
 BUILD_DIR=/data/openpilot
 SOURCE_DIR="$(git rev-parse --show-toplevel)"
 
-BRANCH=release3-staging
+if [ -f /TICI ]; then
+  FILES_SRC="release/files_tici"
+  RELEASE_BRANCH=release3-staging
+  DASHCAM_BRANCH=dashcam3-staging
+elif [ -f /EON ]; then
+  FILES_SRC="release/files_eon"
+  RELEASE_BRANCH=release2-staging
+  DASHCAM_BRANCH=dashcam-staging
+else
+  exit 0
+fi
 
 # set git identity
 source $DIR/identity.sh
@@ -20,13 +30,14 @@ mkdir -p $BUILD_DIR
 cd $BUILD_DIR
 git init
 git remote add origin git@github.com:commaai/openpilot.git
-git checkout -f -B $BRANCH
+git fetch origin $RELEASE_BRANCH
+git checkout --orphan $RELEASE_BRANCH
 
 # do the files copy
 echo "[-] copying files T=$SECONDS"
 cd $SOURCE_DIR
 cp -pR --parents $(cat release/files_common) $BUILD_DIR/
-cp -pR --parents $(cat release/files_tici) $BUILD_DIR/
+cp -pR --parents $(cat $FILES_SRC) $BUILD_DIR/
 
 # in the directory
 cd $BUILD_DIR
@@ -39,6 +50,7 @@ echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h
 echo "[-] committing version $VERSION T=$SECONDS"
 git add -f .
 git commit -a -m "openpilot v$VERSION release"
+git branch --set-upstream-to=origin/$RELEASE_BRANCH
 
 # Build panda firmware
 pushd panda/
@@ -50,15 +62,20 @@ popd
 export PYTHONPATH="$BUILD_DIR"
 scons -j$(nproc)
 
-# Run tests
-#python selfdrive/manager/test/test_manager.py
-selfdrive/car/tests/test_car_interfaces.py
+# Ensure no submodules in release
+if test "$(git submodule--helper list | wc -l)" -gt "0"; then
+  echo "submodules found:"
+  git submodule--helper list
+  exit 1
+fi
+git submodule status
 
 # Cleanup
 find . -name '*.a' -delete
 find . -name '*.o' -delete
 find . -name '*.os' -delete
 find . -name '*.pyc' -delete
+find . -name 'moc_*' -delete
 find . -name '__pycache__' -delete
 rm -rf panda/board panda/certs panda/crypto
 rm -rf .sconsign.dblite Jenkinsfile release/
@@ -68,8 +85,8 @@ rm models/supercombo.dlc
 mkdir -p panda/board/obj
 mv /tmp/panda.bin.signed panda/board/obj/panda.bin.signed
 
-# Restore phonelibs
-git checkout phonelibs/
+# Restore third_party
+git checkout third_party/
 
 # Mark as prebuilt release
 touch prebuilt
@@ -78,15 +95,24 @@ touch prebuilt
 git add -f .
 git commit --amend -m "openpilot v$VERSION"
 
+# Run tests
+TEST_FILES="tools/"
+cd $SOURCE_DIR
+cp -pR -n --parents $TEST_FILES $BUILD_DIR/
+cd $BUILD_DIR
+RELEASE=1 selfdrive/test/test_onroad.py
+#selfdrive/manager/test/test_manager.py
+selfdrive/car/tests/test_car_interfaces.py
+rm -rf $TEST_FILES
+
 if [ ! -z "$PUSH" ]; then
   echo "[-] pushing T=$SECONDS"
-  git remote set-url origin git@github.com:commaai/openpilot.git
-  git push -f origin $BRANCH
+  git push -f origin $RELEASE_BRANCH
 
   # Create dashcam
   git rm selfdrive/car/*/carcontroller.py
   git commit -m "create dashcam release from release"
-  git push -f origin $BRANCH:dashcam3-staging
+  git push -f origin $RELEASE_BRANCH:$DASHCAM_BRANCH
 fi
 
 echo "[-] done T=$SECONDS"
diff --git a/release/build_release2.sh b/release/build_release2.sh
deleted file mode 100755
index c607aa820..000000000
--- a/release/build_release2.sh
+++ /dev/null
@@ -1,104 +0,0 @@
-#!/usr/bin/env bash
-set -e
-
-export GIT_COMMITTER_NAME="Vehicle Researcher"
-export GIT_COMMITTER_EMAIL="user@comma.ai"
-export GIT_AUTHOR_NAME="Vehicle Researcher"
-export GIT_AUTHOR_EMAIL="user@comma.ai"
-
-export GIT_SSH_COMMAND="ssh -i /data/gitkey"
-
-# set CLEAN to build outside of CI
-if [ ! -z "$CLEAN" ]; then
-  # Create folders
-  rm -rf /data/openpilot
-  mkdir -p /data/openpilot
-  cd /data/openpilot
-
-  # Create git repo
-  git init
-  git remote add origin git@github.com:commaai/openpilot.git
-  git fetch origin devel-staging
-else
-  cd /data/openpilot
-  git clean -xdf
-  git branch -D release2-staging || true
-fi
-
-git fetch origin release2-staging
-git fetch origin dashcam-staging
-
-# Create release2 with no history
-if [ ! -z "$CLEAN" ]; then
-  git checkout --orphan release2-staging origin/devel-staging
-else
-  git checkout --orphan release2-staging
-fi
-
-VERSION=$(cat selfdrive/common/version.h | awk -F[\"-]  '{print $2}')
-echo "#define COMMA_VERSION \"$VERSION-release\"" > selfdrive/common/version.h
-
-git commit -m "openpilot v$VERSION"
-
-# Build signed panda firmware
-pushd panda/
-CERT=/tmp/pandaextra/certs/release RELEASE=1 scons -u .
-mv board/obj/panda.bin.signed /tmp/panda.bin.signed
-popd
-
-# Build stuff
-ln -sfn /data/openpilot /data/pythonpath
-export PYTHONPATH="/data/openpilot:/data/openpilot/pyextra"
-scons -j3
-
-# Run tests
-python selfdrive/manager/test/test_manager.py
-selfdrive/car/tests/test_car_interfaces.py
-
-# Ensure no submodules in release
-if test "$(git submodule--helper list | wc -l)" -gt "0"; then
-  echo "submodules found:"
-  git submodule--helper list
-  exit 1
-fi
-git submodule status
-
-# Cleanup
-find . -name '*.a' -delete
-find . -name '*.o' -delete
-find . -name '*.os' -delete
-find . -name '*.pyc' -delete
-find . -name '__pycache__' -delete
-rm -rf panda/board panda/certs panda/crypto
-rm -rf .sconsign.dblite Jenkinsfile release/
-rm models/supercombo.dlc
-
-# Move back signed panda fw
-mkdir -p panda/board/obj
-mv /tmp/panda.bin.signed panda/board/obj/panda.bin.signed
-
-# Restore phonelibs
-git checkout phonelibs/
-
-# Mark as prebuilt release
-touch prebuilt
-
-# Add built files to git
-git add -f .
-git commit --amend -m "openpilot v$VERSION"
-
-# Print committed files that are normally gitignored
-#git status --ignored
-
-if [ ! -z "$PUSH" ]; then
-  git remote set-url origin git@github.com:commaai/openpilot.git
-
-  # Push to release2-staging
-  git push -f origin release2-staging
-
-  # Create dashcam release
-  git rm selfdrive/car/*/carcontroller.py
-
-  git commit -m "create dashcam release from release2"
-  git push -f origin release2-staging:dashcam-staging
-fi
diff --git a/release/files_common b/release/files_common
index a025e661b..68c2600c2 100644
--- a/release/files_common
+++ b/release/files_common
@@ -7,10 +7,9 @@ launch_openpilot.sh
 Jenkinsfile
 SConstruct
 
-CONTRIBUTING.md
 README.md
 RELEASES.md
-SAFETY.md
+docs/*
 site_scons/site_tools/cython.py
 
 common/.gitignore
@@ -61,6 +60,8 @@ models/dmonitoring_model_q.dlc
 
 release/*
 
+installer/updater/updater
+
 selfdrive/version.py
 
 selfdrive/__init__.py
@@ -96,6 +97,7 @@ selfdrive/car/car_helpers.py
 selfdrive/car/fingerprints.py
 selfdrive/car/interfaces.py
 selfdrive/car/vin.py
+selfdrive/car/disable_ecu.py
 selfdrive/car/fw_versions.py
 selfdrive/car/isotp_parallel_query.py
 selfdrive/car/tests/__init__.py
@@ -188,11 +190,6 @@ selfdrive/debug/*.py
 selfdrive/common/SConscript
 selfdrive/common/version.h
 
-selfdrive/common/framebuffer.h
-selfdrive/common/framebuffer.cc
-selfdrive/common/glutil.cc
-selfdrive/common/glutil.h
-selfdrive/common/touch.[c,h]
 selfdrive/common/swaglog.h
 selfdrive/common/swaglog.cc
 selfdrive/common/util.cc
@@ -238,44 +235,25 @@ selfdrive/controls/lib/pid.py
 selfdrive/controls/lib/longitudinal_planner.py
 selfdrive/controls/lib/radar_helpers.py
 selfdrive/controls/lib/vehicle_model.py
-selfdrive/controls/lib/fcw.py
-selfdrive/controls/lib/long_mpc.py
-selfdrive/controls/lib/lead_mpc.py
 
 selfdrive/controls/lib/cluster/*
 
-selfdrive/controls/lib/lateral_mpc/lib_mpc_export/*
-selfdrive/controls/lib/lateral_mpc/.gitignore
-selfdrive/controls/lib/lateral_mpc/SConscript
-selfdrive/controls/lib/lateral_mpc/__init__.py
-selfdrive/controls/lib/lateral_mpc/generator.cpp
-selfdrive/controls/lib/lateral_mpc/libmpc_py.py
-selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
-
-selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/*
-selfdrive/controls/lib/lead_mpc_lib/.gitignore
-selfdrive/controls/lib/lead_mpc_lib/SConscript
-selfdrive/controls/lib/lead_mpc_lib/__init__.py
-selfdrive/controls/lib/lead_mpc_lib/generator.cpp
-selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py
-selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c
-
-selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/*
+selfdrive/controls/lib/lateral_mpc_lib/.gitignore
 selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
-selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
-selfdrive/controls/lib/longitudinal_mpc_lib/__init__.py
-selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp
-selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py
-selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c
+selfdrive/controls/lib/lateral_mpc_lib/*
+selfdrive/controls/lib/longitudinal_mpc_lib/*
 
 selfdrive/hardware/__init__.py
 selfdrive/hardware/base.h
 selfdrive/hardware/base.py
 selfdrive/hardware/hw.h
 selfdrive/hardware/eon/__init__.py
+selfdrive/hardware/eon/androidd.py
 selfdrive/hardware/eon/hardware.h
 selfdrive/hardware/eon/hardware.py
-selfdrive/hardware/eon/androidd.py
+selfdrive/hardware/eon/neos.py
+selfdrive/hardware/eon/neos.json
+selfdrive/hardware/eon/updater
 selfdrive/hardware/tici/__init__.py
 selfdrive/hardware/tici/hardware.py
 selfdrive/hardware/tici/amplifier.py
@@ -369,6 +347,9 @@ selfdrive/ui/qt/widgets/*.h
 selfdrive/ui/qt/spinner_aarch64
 selfdrive/ui/qt/text_aarch64
 
+selfdrive/ui/replay/framereader.cc
+selfdrive/ui/replay/framereader.h
+
 selfdrive/camerad/SConscript
 selfdrive/camerad/main.cc
 
@@ -380,6 +361,8 @@ selfdrive/camerad/cameras/camera_frame_stream.cc
 selfdrive/camerad/cameras/camera_frame_stream.h
 selfdrive/camerad/cameras/camera_qcom.cc
 selfdrive/camerad/cameras/camera_qcom.h
+selfdrive/camerad/cameras/camera_replay.cc
+selfdrive/camerad/cameras/camera_replay.h
 selfdrive/camerad/cameras/debayer.cl
 selfdrive/camerad/cameras/sensor_i2c.h
 selfdrive/camerad/cameras/sensor2_i2c.h
@@ -444,56 +427,56 @@ selfdrive/assets/assets.qrc
 selfdrive/assets/*.png
 selfdrive/assets/*.svg
 selfdrive/assets/fonts/*.ttf
+selfdrive/assets/icons/*
 selfdrive/assets/images/*
 selfdrive/assets/offroad/*
 selfdrive/assets/sounds/*
 selfdrive/assets/training/*
 
-phonelibs/SConscript
+third_party/SConscript
 
-phonelibs/nanovg/*.c
-phonelibs/nanovg/*.h
+third_party/nanovg/*.c
+third_party/nanovg/*.h
 
-phonelibs/libgralloc/**
-phonelibs/linux/**
-phonelibs/opencl/**
-phonelibs/zlib/*
-phonelibs/bzip2/*
-phonelibs/openmax/**
+third_party/libgralloc/**
+third_party/linux/**
+third_party/opencl/**
+third_party/zlib/*
+third_party/bzip2/*
+third_party/openmax/**
 
-phonelibs/json11/json11.cpp
-phonelibs/json11/json11.hpp
+third_party/json11/json11.cpp
+third_party/json11/json11.hpp
 
-phonelibs/qpoases/**
+third_party/qrcode/*.cc
+third_party/qrcode/*.hpp
 
-phonelibs/qrcode/*.cc
-phonelibs/qrcode/*.hpp
+third_party/kaitai/*.h
+third_party/kaitai/*.cpp
 
-phonelibs/kaitai/*.h
-phonelibs/kaitai/*.cpp
+third_party/libyuv/include/**
+third_party/libyuv/lib/**
+third_party/libyuv/larch64/**
 
-phonelibs/libyuv/include/**
-phonelibs/libyuv/lib/**
-phonelibs/libyuv/larch64/**
+third_party/snpe/include/**
+third_party/snpe/aarch64**
+third_party/snpe/larch64**
+third_party/snpe/dsp**
 
-phonelibs/snpe/include/**
-phonelibs/snpe/aarch64**
-phonelibs/snpe/larch64**
-phonelibs/snpe/dsp**
+third_party/acados/x86_64/**
+third_party/acados/aarch64/**
+third_party/acados/larch64/**
+third_party/acados/include/**
 
-phonelibs/android_frameworks_native/**
-phonelibs/android_hardware_libhardware/**
-phonelibs/android_system_core/**
-
-installer/updater/updater
-installer/updater/updater.cc
-installer/updater/update.json
-installer/updater/Makefile
+third_party/android_frameworks_native/**
+third_party/android_hardware_libhardware/**
+third_party/android_system_core/**
 
 scripts/update_now.sh
 scripts/stop_updater.sh
 
 pyextra/.gitignore
+pyextra/acados_template/**
 
 rednose/**
 
@@ -588,6 +571,7 @@ opendbc/honda_insight_ex_2019_can_generated.dbc
 opendbc/acura_ilx_2016_nidec.dbc
 
 opendbc/hyundai_kia_generic.dbc
+opendbc/hyundai_kia_mando_front_radar.dbc
 
 opendbc/mazda_2017.dbc
 
diff --git a/release/files_eon b/release/files_eon
new file mode 100644
index 000000000..b43bf86b5
--- /dev/null
+++ b/release/files_eon
@@ -0,0 +1 @@
+README.md
diff --git a/release/files_pc b/release/files_pc
index 1b57f97a7..2f55599c2 100644
--- a/release/files_pc
+++ b/release/files_pc
@@ -1,3 +1,3 @@
-phonelibs/mapbox-gl-native-qt/x86_64/**
+third_party/mapbox-gl-native-qt/x86_64/**
 
-phonelibs/qt-plugins/x86_64/**
+third_party/qt-plugins/x86_64/**
diff --git a/release/files_tici b/release/files_tici
index cf5184a58..b5a2e5b7a 100644
--- a/release/files_tici
+++ b/release/files_tici
@@ -1,4 +1,4 @@
-phonelibs/mapbox-gl-native-qt/include/*
+third_party/mapbox-gl-native-qt/include/*
 
 selfdrive/timezoned.py
 
diff --git a/selfdrive/assets/assets.qrc b/selfdrive/assets/assets.qrc
index 1fc6d3591..39be41aa6 100644
--- a/selfdrive/assets/assets.qrc
+++ b/selfdrive/assets/assets.qrc
@@ -5,8 +5,10 @@
   <file>img_circled_slash.svg</file>
   <file>img_eye_open.svg</file>
   <file>img_eye_closed.svg</file>
+  <file>icons/close.svg</file>
   <file>offroad/icon_lock_closed.svg</file>
   <file>offroad/icon_checkmark.svg</file>
+  <file>offroad/icon_warning.png</file>
   <file>offroad/icon_wifi_strength_low.svg</file>
   <file>offroad/icon_wifi_strength_medium.svg</file>
   <file>offroad/icon_wifi_strength_high.svg</file>
diff --git a/selfdrive/assets/fonts/courbd.ttf b/selfdrive/assets/fonts/courbd.ttf
deleted file mode 100644
index a4f26b4b5..000000000
Binary files a/selfdrive/assets/fonts/courbd.ttf and /dev/null differ
diff --git a/selfdrive/assets/icons/close.svg b/selfdrive/assets/icons/close.svg
new file mode 100644
index 000000000..b1e6d3b86
--- /dev/null
+++ b/selfdrive/assets/icons/close.svg
@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<svg id="svg4" width="63.238" height="63.238" fill="none" version="1.1" viewBox="0 0 63.238 63.238" xmlns="http://www.w3.org/2000/svg">
+ <path id="path2" d="m 55.7181,63.2381 7.52,-7.52 L 39.1199,31.6 63.219,7.5009 55.7371,0.019 31.6381,24.1181 7.5199,0 0,7.52 24.1181,31.6381 0.019,55.7372 7.5009,63.219 31.6,39.12 Z" fill="#000"/>
+</svg>
diff --git a/selfdrive/assets/images/battery.png b/selfdrive/assets/images/battery.png
deleted file mode 100644
index cac3b9096..000000000
Binary files a/selfdrive/assets/images/battery.png and /dev/null differ
diff --git a/selfdrive/assets/images/battery_charging.png b/selfdrive/assets/images/battery_charging.png
deleted file mode 100644
index 4c25566d0..000000000
Binary files a/selfdrive/assets/images/battery_charging.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficLight_green.png b/selfdrive/assets/img_trafficLight_green.png
deleted file mode 100644
index b2e07dab0..000000000
Binary files a/selfdrive/assets/img_trafficLight_green.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficLight_none.png b/selfdrive/assets/img_trafficLight_none.png
deleted file mode 100644
index 01deb5a00..000000000
Binary files a/selfdrive/assets/img_trafficLight_none.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficLight_red.png b/selfdrive/assets/img_trafficLight_red.png
deleted file mode 100644
index 4c440250e..000000000
Binary files a/selfdrive/assets/img_trafficLight_red.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficLight_yellow.png b/selfdrive/assets/img_trafficLight_yellow.png
deleted file mode 100644
index 8dd5bd42c..000000000
Binary files a/selfdrive/assets/img_trafficLight_yellow.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficSign_stop.png b/selfdrive/assets/img_trafficSign_stop.png
deleted file mode 100644
index 9261c4747..000000000
Binary files a/selfdrive/assets/img_trafficSign_stop.png and /dev/null differ
diff --git a/selfdrive/assets/img_trafficSign_turn.png b/selfdrive/assets/img_trafficSign_turn.png
deleted file mode 100644
index c304d1bf2..000000000
Binary files a/selfdrive/assets/img_trafficSign_turn.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_app_store.png b/selfdrive/assets/offroad/icon_app_store.png
deleted file mode 100644
index ae0dd95ce..000000000
Binary files a/selfdrive/assets/offroad/icon_app_store.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_connect_app.png b/selfdrive/assets/offroad/icon_connect_app.png
deleted file mode 100644
index cd216d3e6..000000000
Binary files a/selfdrive/assets/offroad/icon_connect_app.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_eon.png b/selfdrive/assets/offroad/icon_eon.png
deleted file mode 100644
index 72856c4e6..000000000
Binary files a/selfdrive/assets/offroad/icon_eon.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_play_store.png b/selfdrive/assets/offroad/icon_play_store.png
deleted file mode 100644
index 1eca9d589..000000000
Binary files a/selfdrive/assets/offroad/icon_play_store.png and /dev/null differ
diff --git a/selfdrive/assets/offroad/icon_user.png b/selfdrive/assets/offroad/icon_user.png
deleted file mode 100644
index 9b653cc4b..000000000
Binary files a/selfdrive/assets/offroad/icon_user.png and /dev/null differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 4c656756b..fadbad0bf 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -253,7 +253,7 @@ def uploadFileToUrl(fn, url, headers):
 @dispatcher.add_method
 def listUploadQueue():
   items = list(upload_queue.queue) + list(cur_upload_items.values())
-  return [i._asdict() for i in items if i is not None]
+  return [i._asdict() for i in items if (i is not None) and (i.id not in cancelled_uploads)]
 
 
 @dispatcher.add_method
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 3f09ec949..edf5e5e8e 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -9,9 +9,11 @@
 #include <bitset>
 #include <cassert>
 #include <cerrno>
+#include <chrono>
 #include <cstdint>
 #include <cstdio>
 #include <cstdlib>
+#include <future>
 #include <thread>
 #include <unordered_map>
 
@@ -34,13 +36,19 @@
 #define CUTOFF_IL 200
 #define SATURATE_IL 1600
 #define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a')
+using namespace std::chrono_literals;
 
-std::atomic<bool> safety_setter_thread_running(false);
 std::atomic<bool> ignition(false);
 
 ExitHandler do_exit;
 
-void safety_setter_thread(Panda *panda) {
+std::string get_time_str(const struct tm &time) {
+  char s[30] = {'\0'};
+  std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time);
+  return s;
+}
+
+bool safety_setter_thread(Panda *panda) {
   LOGD("Starting safety setter thread");
   // diagnostic only is the default, needed for VIN query
   panda->set_safety_model(cereal::CarParams::SafetyModel::ELM327);
@@ -49,9 +57,8 @@ void safety_setter_thread(Panda *panda) {
 
   // switch to SILENT when CarVin param is read
   while (true) {
-    if (do_exit || !panda->connected) {
-      safety_setter_thread_running = false;
-      return;
+    if (do_exit || !panda->connected || !ignition) {
+      return false;
     };
 
     std::string value_vin = p.get("CarVin");
@@ -61,7 +68,7 @@ void safety_setter_thread(Panda *panda) {
       LOGW("got CarVin %s", value_vin.c_str());
       break;
     }
-    util::sleep_for(100);
+    util::sleep_for(20);
   }
 
   // VIN query done, stop listening to OBDII
@@ -70,9 +77,8 @@ void safety_setter_thread(Panda *panda) {
   std::string params;
   LOGW("waiting for params to set safety model");
   while (true) {
-    if (do_exit || !panda->connected) {
-      safety_setter_thread_running = false;
-      return;
+    if (do_exit || !panda->connected || !ignition) {
+      return false;
     };
 
     if (p.getBool("ControlsReady")) {
@@ -86,16 +92,24 @@ void safety_setter_thread(Panda *panda) {
   AlignedBuffer aligned_buf;
   capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
   cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
-  cereal::CarParams::SafetyModel safety_model = car_params.getSafetyModel();
+  cereal::CarParams::SafetyModel safety_model;
+  int safety_param;
+
+  auto safety_configs = car_params.getSafetyConfigs();
+  if (safety_configs.size() > 0) {
+    safety_model = safety_configs[0].getSafetyModel();
+    safety_param = safety_configs[0].getSafetyParam();
+  } else {
+    // If no safety mode is set, default to silent
+    safety_model = cereal::CarParams::SafetyModel::SILENT;
+    safety_param = 0;
+  }
 
   panda->set_unsafe_mode(0);  // see safety_declarations.h for allowed values
 
-  auto safety_param = car_params.getSafetyParam();
   LOGW("setting safety model: %d with param %d", (int)safety_model, safety_param);
-
   panda->set_safety_model(safety_model, safety_param);
-
-  safety_setter_thread_running = false;
+  return true;
 }
 
 
@@ -137,7 +151,7 @@ Panda *usb_connect() {
   // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
 #ifndef __x86_64__
   static std::once_flag connected_once;
-  std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PandaState::UsbPowerMode::CDP);
+  std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PeripheralState::UsbPowerMode::CDP);
 #endif
 
   if (panda->has_rtc) {
@@ -146,13 +160,8 @@ Panda *usb_connect() {
     struct tm rtc_time = panda->get_rtc();
 
     if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
-      LOGE("System time wrong, setting from RTC. "
-           "System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d",
-           sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday,
-           sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec,
-           rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday,
-           rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec);
-
+      LOGE("System time wrong, setting from RTC. System: %s RTC: %s",
+           get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
       const struct timeval tv = {mktime(&rtc_time), 0};
       settimeofday(&tv, 0);
     }
@@ -161,20 +170,6 @@ Panda *usb_connect() {
   return panda.release();
 }
 
-// must be called before threads or with mutex
-static Panda *usb_retry_connect() {
-  LOGW("attempting to connect");
-  while (!do_exit) {
-    Panda *panda = usb_connect();
-    if (panda) {
-      LOGW("connected to board");
-      return panda;
-    }
-    util::sleep_for(100); 
-  };
-  return nullptr;
-}
-
 void can_recv(Panda *panda, PubMaster &pm) {
   kj::Array<capnp::word> can_data;
   panda->can_receive(can_data);
@@ -247,64 +242,135 @@ void can_recv_thread(Panda *panda) {
   }
 }
 
-void panda_state_thread(Panda *&panda, bool spoofing_started) {
-  LOGD("start panda state thread");
-  PubMaster pm({"pandaState"});
+void send_empty_peripheral_state(PubMaster *pm) {
+  MessageBuilder msg;
+  auto peripheralState  = msg.initEvent().initPeripheralState();
+  peripheralState.setPandaType(cereal::PandaState::PandaType::UNKNOWN);
+  pm->send("peripheralState", msg);
+}
 
-  uint32_t no_ignition_cnt = 0;
-  bool ignition_last = false;
-  Params params = Params();
+void send_empty_panda_state(PubMaster *pm) {
+  MessageBuilder msg;
+  auto pandaStates = msg.initEvent().initPandaStates(1);
+  pandaStates[0].setPandaType(cereal::PandaState::PandaType::UNKNOWN);
+  pm->send("pandaStates", msg);
+}
 
-  // Broadcast empty pandaState message when panda is not yet connected
-  while (!do_exit && !panda) {
-    MessageBuilder msg;
-    auto pandaState  = msg.initEvent().initPandaState();
+bool send_panda_state(PubMaster *pm, Panda *panda, bool spoofing_started) {
+  health_t pandaState = panda->get_state();
 
-    pandaState.setPandaType(cereal::PandaState::PandaType::UNKNOWN);
-    pm.send("pandaState", msg);
-    util::sleep_for(500);
+  if (spoofing_started) {
+    pandaState.ignition_line = 1;
   }
 
+  // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
+  if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+    panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+  }
+
+  bool ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
+
+#ifndef __x86_64__
+  bool power_save_desired = !ignition;
+  if (pandaState.power_save_enabled != power_save_desired) {
+    panda->set_power_saving(power_save_desired);
+  }
+
+  // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
+  if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+    panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+  }
+#endif
+
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  evt.setValid(panda->comms_healthy);
+
+  // TODO: this has to be adapted to merge in multipanda support
+  auto ps = evt.initPandaStates(1);
+  ps[0].setUptime(pandaState.uptime);
+  ps[0].setIgnitionLine(pandaState.ignition_line);
+  ps[0].setIgnitionCan(pandaState.ignition_can);
+  ps[0].setControlsAllowed(pandaState.controls_allowed);
+  ps[0].setGasInterceptorDetected(pandaState.gas_interceptor_detected);
+  ps[0].setCanRxErrs(pandaState.can_rx_errs);
+  ps[0].setCanSendErrs(pandaState.can_send_errs);
+  ps[0].setCanFwdErrs(pandaState.can_fwd_errs);
+  ps[0].setGmlanSendErrs(pandaState.gmlan_send_errs);
+  ps[0].setPandaType(panda->hw_type);
+  ps[0].setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
+  ps[0].setSafetyParam(pandaState.safety_param);
+  ps[0].setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
+  ps[0].setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
+  ps[0].setHeartbeatLost((bool)(pandaState.heartbeat_lost));
+  ps[0].setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
+
+  // Convert faults bitset to capnp list
+  std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
+  auto faults = ps[0].initFaults(fault_bits.count());
+
+  size_t i = 0;
+  for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
+      f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
+    if (fault_bits.test(f)) {
+      faults.set(i, cereal::PandaState::FaultType(f));
+      i++;
+    }
+  }
+  pm->send("pandaStates", msg);
+
+  return ignition;
+}
+
+void send_peripheral_state(PubMaster *pm, Panda *panda) {
+  health_t pandaState = panda->get_state();
+
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  evt.setValid(panda->comms_healthy);
+
+  auto ps = evt.initPeripheralState();
+  ps.setPandaType(panda->hw_type);
+
+  if (Hardware::TICI()) {
+    double read_time = millis_since_boot();
+    ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()));
+    ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()));
+    read_time = millis_since_boot() - read_time;
+    if (read_time > 50) {
+      LOGW("reading hwmon took %lfms", read_time);
+    }
+  } else {
+    ps.setVoltage(pandaState.voltage);
+    ps.setCurrent(pandaState.current);
+  }
+
+  uint16_t fan_speed_rpm = panda->get_fan_speed();
+  ps.setUsbPowerMode(cereal::PeripheralState::UsbPowerMode(pandaState.usb_power_mode));
+  ps.setFanSpeedRpm(fan_speed_rpm);
+
+  pm->send("peripheralState", msg);
+}
+
+void panda_state_thread(PubMaster *pm, Panda * peripheral_panda, Panda *panda, bool spoofing_started) {
+  Params params;
+  bool ignition_last = false;
+  std::future<bool> safety_future;
+
+  LOGD("start panda state thread");
+
   // run at 2hz
   while (!do_exit && panda->connected) {
-    health_t pandaState = panda->get_state();
-
-    if (spoofing_started) {
-      pandaState.ignition_line = 1;
-    }
-
-    // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
-    if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
-      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
-    }
-
-    ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
-
-    if (ignition) {
-      no_ignition_cnt = 0;
-    } else {
-      no_ignition_cnt += 1;
-    }
-
-#ifndef __x86_64__
-    bool power_save_desired = !ignition;
-    if (pandaState.power_save_enabled != power_save_desired) {
-      panda->set_power_saving(power_save_desired);
-    }
-
-    // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
-    if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
-      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
-    }
-#endif
+    send_peripheral_state(pm, peripheral_panda);
+    ignition = send_panda_state(pm, panda, spoofing_started);
 
     // clear VIN, CarParams, and set new safety on car start
     if (ignition && !ignition_last) {
       params.clearAll(CLEAR_ON_IGNITION_ON);
-
-      if (!safety_setter_thread_running) {
-        safety_setter_thread_running = true;
-        std::thread(safety_setter_thread, panda).detach();
+      if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
+        safety_future = std::async(std::launch::async, safety_setter_thread, panda);
       } else {
         LOGW("Safety setter thread already running");
       }
@@ -312,92 +378,16 @@ void panda_state_thread(Panda *&panda, bool spoofing_started) {
       params.clearAll(CLEAR_ON_IGNITION_OFF);
     }
 
-    // Write to rtc once per minute when no ignition present
-    if ((panda->has_rtc) && !ignition && (no_ignition_cnt % 120 == 1)) {
-      // Write time to RTC if it looks reasonable
-      setenv("TZ","UTC",1);
-      struct tm sys_time = util::get_time();
-
-      if (util::time_valid(sys_time)) {
-        struct tm rtc_time = panda->get_rtc();
-        double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
-
-        if (std::abs(seconds) > 1.1) {
-          panda->set_rtc(sys_time);
-          LOGW("Updating panda RTC. dt = %.2f "
-               "System: %d-%02d-%02d %02d:%02d:%02d RTC: %d-%02d-%02d %02d:%02d:%02d",
-               seconds,
-               sys_time.tm_year + 1900, sys_time.tm_mon + 1, sys_time.tm_mday,
-               sys_time.tm_hour, sys_time.tm_min, sys_time.tm_sec,
-               rtc_time.tm_year + 1900, rtc_time.tm_mon + 1, rtc_time.tm_mday,
-               rtc_time.tm_hour, rtc_time.tm_min, rtc_time.tm_sec);
-        }
-      }
-    }
-
     ignition_last = ignition;
-    uint16_t fan_speed_rpm = panda->get_fan_speed();
 
-    // build msg
-    MessageBuilder msg;
-    auto evt = msg.initEvent();
-    evt.setValid(panda->comms_healthy);
-
-    auto ps = evt.initPandaState();
-    ps.setUptime(pandaState.uptime);
-
-    if (Hardware::TICI()) {
-      double read_time = millis_since_boot();
-      ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()));
-      ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()));
-      read_time = millis_since_boot() - read_time;
-      if (read_time > 50) {
-        LOGW("reading hwmon took %lfms", read_time);
-      }
-    } else {
-      ps.setVoltage(pandaState.voltage);
-      ps.setCurrent(pandaState.current);
-    }
-
-    ps.setIgnitionLine(pandaState.ignition_line);
-    ps.setIgnitionCan(pandaState.ignition_can);
-    ps.setControlsAllowed(pandaState.controls_allowed);
-    ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
-    ps.setHasGps(true);
-    ps.setCanRxErrs(pandaState.can_rx_errs);
-    ps.setCanSendErrs(pandaState.can_send_errs);
-    ps.setCanFwdErrs(pandaState.can_fwd_errs);
-    ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
-    ps.setPandaType(panda->hw_type);
-    ps.setUsbPowerMode(cereal::PandaState::UsbPowerMode(pandaState.usb_power_mode));
-    ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
-    ps.setSafetyParam(pandaState.safety_param);
-    ps.setFanSpeedRpm(fan_speed_rpm);
-    ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
-    ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
-    ps.setHeartbeatLost((bool)(pandaState.heartbeat_lost));
-    ps.setHarnessStatus(cereal::PandaState::HarnessStatus(pandaState.car_harness_status));
-
-    // Convert faults bitset to capnp list
-    std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
-    auto faults = ps.initFaults(fault_bits.count());
-
-    size_t i = 0;
-    for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
-        f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TICK); f++) {
-      if (fault_bits.test(f)) {
-        faults.set(i, cereal::PandaState::FaultType(f));
-        i++;
-      }
-    }
-    pm.send("pandaState", msg);
     panda->send_heartbeat();
     util::sleep_for(500);
   }
 }
 
-void hardware_control_thread(Panda *panda) {
-  LOGD("start hardware control thread");
+
+void peripheral_control_thread(Panda *panda) {
+  LOGD("start peripheral control thread");
   SubMaster sm({"deviceState", "driverCameraState"});
 
   uint64_t last_front_frame_t = 0;
@@ -418,10 +408,10 @@ void hardware_control_thread(Panda *panda) {
       bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled();
       if (charging_disabled != prev_charging_disabled) {
         if (charging_disabled) {
-          panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CLIENT);
+          panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CLIENT);
           LOGW("TURN OFF CHARGING!\n");
         } else {
-          panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP);
+          panda->set_usb_power_mode(cereal::PeripheralState::UsbPowerMode::CDP);
           LOGW("TURN ON CHARGING!\n");
         }
         prev_charging_disabled = charging_disabled;
@@ -467,6 +457,23 @@ void hardware_control_thread(Panda *panda) {
       prev_ir_pwr = ir_pwr;
     }
 
+    // Write to rtc once per minute when no ignition present
+    if ((panda->has_rtc) && !ignition && (cnt % 120 == 1)) {
+      // Write time to RTC if it looks reasonable
+      setenv("TZ","UTC",1);
+      struct tm sys_time = util::get_time();
+
+      if (util::time_valid(sys_time)) {
+        struct tm rtc_time = panda->get_rtc();
+        double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
+
+        if (std::abs(seconds) > 1.1) {
+          panda->set_rtc(sys_time);
+          LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
+                seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+        }
+      }
+    }
   }
 }
 
@@ -508,7 +515,7 @@ void pigeon_thread(Panda *panda) {
     for (const auto& [msg_cls, max_dt] : cls_max_dt) {
       int64_t dt = (int64_t)nanos_since_boot() - (int64_t)last_recv_time[msg_cls];
       if (ignition_last && ignition && dt > max_dt) {
-        LOG("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt);
+        LOGD("ublox receive timeout, msg class: 0x%02x, dt %llu", msg_cls, dt);
         // TODO: turn on reset after verification of logs
         // need_reset = true;
       }
@@ -557,23 +564,34 @@ int main() {
   int err = set_realtime_priority(54);
   LOG("set priority returns %d", err);
 
-  err = set_core_affinity(Hardware::TICI() ? 4 : 3);
+  err = set_core_affinity({Hardware::TICI() ? 4 : 3});
   LOG("set affinity returns %d", err);
 
-  while (!do_exit) {
-    Panda *panda = nullptr;
-    std::vector<std::thread> threads;
-    threads.emplace_back(panda_state_thread, std::ref(panda), getenv("STARTED") != nullptr);
+  LOGW("attempting to connect");
+  PubMaster pm({"pandaStates", "peripheralState"});
 
-    // connect to the board
-    panda = usb_retry_connect();
-    if (panda != nullptr) {
-      threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
-      threads.emplace_back(can_recv_thread, panda);
-      threads.emplace_back(hardware_control_thread, panda);
-      threads.emplace_back(pigeon_thread, panda);
+  while (!do_exit) {
+    Panda *panda = usb_connect();
+    Panda *peripheral_panda = panda;
+
+    // Send empty pandaState & peripheralState and try again
+    if (panda == nullptr || peripheral_panda == nullptr) {
+      send_empty_panda_state(&pm);
+      send_empty_peripheral_state(&pm);
+      util::sleep_for(500);
+      continue;
     }
 
+    LOGW("connected to board");
+
+    std::vector<std::thread> threads;
+    threads.emplace_back(panda_state_thread, &pm, peripheral_panda, panda, getenv("STARTED") != nullptr);
+    threads.emplace_back(peripheral_control_thread, peripheral_panda);
+    threads.emplace_back(pigeon_thread, peripheral_panda);
+
+    threads.emplace_back(can_send_thread, panda, getenv("FAKESEND") != nullptr);
+    threads.emplace_back(can_recv_thread, panda);
+
     for (auto &t : threads) t.join();
 
     delete panda;
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 7c78c8768..c123d0c80 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -11,17 +11,19 @@
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/util.h"
 
-static int init_usb_ctx(libusb_context *context) {
-  int err = libusb_init(&context);
+static int init_usb_ctx(libusb_context **context) {
+  assert(context != nullptr);
+
+  int err = libusb_init(context);
   if (err != 0) {
     LOGE("libusb initialization error");
     return err;
   }
 
 #if LIBUSB_API_VERSION >= 0x01000106
-  libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
+  libusb_set_option(*context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
 #else
-  libusb_set_debug(context, 3);
+  libusb_set_debug(*context, 3);
 #endif
 
   return err;
@@ -32,7 +34,7 @@ Panda::Panda(std::string serial) {
   // init libusb
   ssize_t num_devices;
   libusb_device **dev_list = NULL;
-  int err = init_usb_ctx(ctx);
+  int err = init_usb_ctx(&ctx);
   if (err != 0) { goto fail; }
 
   // connect by serial
@@ -113,7 +115,7 @@ std::vector<std::string> Panda::list() {
   libusb_device **dev_list = NULL;
   std::vector<std::string> serials;
 
-  int err = init_usb_ctx(context);
+  int err = init_usb_ctx(&context);
   if (err != 0) { return serials; }
 
   num_devices = libusb_get_device_list(context, &dev_list);
@@ -333,7 +335,7 @@ void Panda::set_power_saving(bool power_saving) {
   usb_write(0xe7, power_saving, 0);
 }
 
-void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode) {
+void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) {
   usb_write(0xe6, (uint16_t)power_mode, 0);
 }
 
@@ -342,10 +344,12 @@ void Panda::send_heartbeat() {
 }
 
 void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
-  static std::vector<uint32_t> send;
   const int msg_count = can_data_list.size();
+  const int buf_size = msg_count*0x10;
 
-  send.resize(msg_count*0x10);
+  if (send.size() < buf_size) {
+    send.resize(buf_size);
+  }
 
   for (int i = 0; i < msg_count; i++) {
     auto cmsg = can_data_list[i];
@@ -360,7 +364,7 @@ void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list) {
     memcpy(&send[i*4+2], can_data.begin(), can_data.size());
   }
 
-  usb_bulk_write(3, (unsigned char*)send.data(), send.size(), 5);
+  usb_bulk_write(3, (unsigned char*)send.data(), buf_size, 5);
 }
 
 int Panda::can_receive(kj::Array<capnp::word>& out_buf) {
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index dd7600789..36fd699be 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -45,6 +45,7 @@ class Panda {
   libusb_context *ctx = NULL;
   libusb_device_handle *dev_handle = NULL;
   std::mutex usb_lock;
+  std::vector<uint32_t> send;
   void handle_usb_issue(int err, const char func[]);
   void cleanup();
 
@@ -81,7 +82,7 @@ class Panda {
   std::optional<std::vector<uint8_t>> get_firmware_version();
   std::optional<std::string> get_serial();
   void set_power_saving(bool power_saving);
-  void set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode);
+  void set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode);
   void send_heartbeat();
   void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
   int can_receive(kj::Array<capnp::word>& out_buf);
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index c12d25407..65be54aa4 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -1,6 +1,6 @@
-Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM')
+Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM', 'USE_FRAME_STREAM')
 
-libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
+libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
 
 if arch == "aarch64":
   libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui']
@@ -9,15 +9,20 @@ elif arch == "larch64":
   libs += ['atomic']
   cameras = ['cameras/camera_qcom2.cc']
 else:
+  env['CXXFLAGS'] += ["-Wno-deprecated-declarations"]
   if USE_WEBCAM:
     libs += ['opencv_core', 'opencv_highgui', 'opencv_imgproc', 'opencv_videoio']
     cameras = ['cameras/camera_webcam.cc']
     env = env.Clone()
     env.Append(CXXFLAGS = '-DWEBCAM')
     env.Append(CFLAGS = '-DWEBCAM')
-    env.Append(CPPPATH = '/usr/local/include/opencv4')
+    env.Append(CPPPATH = ['/usr/include/opencv4', '/usr/local/include/opencv4'])
   else:
-    cameras = ['cameras/camera_frame_stream.cc']
+    if USE_FRAME_STREAM:
+      cameras = ['cameras/camera_frame_stream.cc']
+    else:
+      libs += ['avutil', 'avcodec', 'avformat', 'swscale']
+      cameras = ['cameras/camera_replay.cc', env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc')]
 
   if arch == "Darwin":
     del libs[libs.index('OpenCL')]
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index 89052e5e1..313d71c60 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -25,7 +25,7 @@
 #elif WEBCAM
 #include "selfdrive/camerad/cameras/camera_webcam.h"
 #else
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
+#include "selfdrive/camerad/cameras/camera_replay.h"
 #endif
 
 const int YUV_COUNT = 100;
@@ -216,66 +216,85 @@ kj::Array<uint8_t> get_frame_image(const CameraBuf *b) {
   return kj::mv(frame_image);
 }
 
-static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
-  uint8_t* thumbnail_buffer = NULL;
-  unsigned long thumbnail_len = 0;
-
-  unsigned char *row = (unsigned char *)malloc(b->rgb_width/4*3);
+static kj::Array<capnp::byte> yuv420_to_jpeg(const CameraBuf *b, int thumbnail_width, int thumbnail_height) {
+  // make the buffer big enough. jpeg_write_raw_data requires 16-pixels aligned height to be used.
+  std::unique_ptr<uint8[]> buf(new uint8_t[(thumbnail_width * ((thumbnail_height + 15) & ~15) * 3) / 2]);
+  uint8_t *y_plane = buf.get();
+  uint8_t *u_plane = y_plane + thumbnail_width * thumbnail_height;
+  uint8_t *v_plane = u_plane + (thumbnail_width * thumbnail_height) / 4;
+  {
+    int result = libyuv::I420Scale(
+        b->cur_yuv_buf->y, b->rgb_width, b->cur_yuv_buf->u, b->rgb_width / 2, b->cur_yuv_buf->v, b->rgb_width / 2,
+        b->rgb_width, b->rgb_height,
+        y_plane, thumbnail_width, u_plane, thumbnail_width / 2, v_plane, thumbnail_width / 2,
+        thumbnail_width, thumbnail_height, libyuv::kFilterNone);
+    if (result != 0) {
+      LOGE("Generate YUV thumbnail failed.");
+      return {};
+    }
+  }
 
   struct jpeg_compress_struct cinfo;
   struct jpeg_error_mgr jerr;
-
   cinfo.err = jpeg_std_error(&jerr);
   jpeg_create_compress(&cinfo);
+
+  uint8_t *thumbnail_buffer = nullptr;
+  size_t thumbnail_len = 0;
   jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len);
 
-  cinfo.image_width = b->rgb_width / 4;
-  cinfo.image_height = b->rgb_height / 4;
+  cinfo.image_width = thumbnail_width;
+  cinfo.image_height = thumbnail_height;
   cinfo.input_components = 3;
-  cinfo.in_color_space = JCS_RGB;
 
   jpeg_set_defaults(&cinfo);
-#ifndef __APPLE__
-  jpeg_set_quality(&cinfo, 50, true);
-  jpeg_start_compress(&cinfo, true);
-#else
-  jpeg_set_quality(&cinfo, 50, static_cast<boolean>(true) );
-  jpeg_start_compress(&cinfo, static_cast<boolean>(true) );
-#endif
+  jpeg_set_colorspace(&cinfo, JCS_YCbCr);
+  // configure sampling factors for yuv420.
+  cinfo.comp_info[0].h_samp_factor = 2;  // Y
+  cinfo.comp_info[0].v_samp_factor = 2;
+  cinfo.comp_info[1].h_samp_factor = 1;  // U
+  cinfo.comp_info[1].v_samp_factor = 1;
+  cinfo.comp_info[2].h_samp_factor = 1;  // V
+  cinfo.comp_info[2].v_samp_factor = 1;
+  cinfo.raw_data_in = TRUE;
 
-  JSAMPROW row_pointer[1];
-  const uint8_t *bgr_ptr = (const uint8_t *)b->cur_rgb_buf->addr;
-  for (int ii = 0; ii < b->rgb_height/4; ii+=1) {
-    for (int j = 0; j < b->rgb_width*3; j+=12) {
-      for (int k = 0; k < 3; k++) {
-        uint16_t dat = 0;
-        int i = ii * 4;
-        dat += bgr_ptr[b->rgb_stride*i + j + k];
-        dat += bgr_ptr[b->rgb_stride*i + j+3 + k];
-        dat += bgr_ptr[b->rgb_stride*(i+1) + j + k];
-        dat += bgr_ptr[b->rgb_stride*(i+1) + j+3 + k];
-        dat += bgr_ptr[b->rgb_stride*(i+2) + j + k];
-        dat += bgr_ptr[b->rgb_stride*(i+2) + j+3 + k];
-        dat += bgr_ptr[b->rgb_stride*(i+3) + j + k];
-        dat += bgr_ptr[b->rgb_stride*(i+3) + j+3 + k];
-        row[(j/4) + (2-k)] = dat/8;
+  jpeg_set_quality(&cinfo, 50, TRUE);
+  jpeg_start_compress(&cinfo, TRUE);
+
+  JSAMPROW y[16], u[8], v[8];
+  JSAMPARRAY planes[3]{y, u, v};
+
+  for (int line = 0; line < cinfo.image_height; line += 16) {
+    for (int i = 0; i < 16; ++i) {
+      y[i] = y_plane + (line + i) * cinfo.image_width;
+      if (i % 2 == 0) {
+        int offset = (cinfo.image_width / 2) * ((i + line) / 2);
+        u[i / 2] = u_plane + offset;
+        v[i / 2] = v_plane + offset;
       }
     }
-    row_pointer[0] = row;
-    jpeg_write_scanlines(&cinfo, row_pointer, 1);
+    jpeg_write_raw_data(&cinfo, planes, 16);
   }
+
   jpeg_finish_compress(&cinfo);
   jpeg_destroy_compress(&cinfo);
-  free(row);
+
+  kj::Array<capnp::byte> dat = kj::heapArray<capnp::byte>(thumbnail_buffer, thumbnail_len);
+  free(thumbnail_buffer);
+  return dat;
+}
+
+static void publish_thumbnail(PubMaster *pm, const CameraBuf *b) {
+  auto thumbnail = yuv420_to_jpeg(b, b->rgb_width / 4, b->rgb_height / 4);
+  if (thumbnail.size() == 0) return;
 
   MessageBuilder msg;
   auto thumbnaild = msg.initEvent().initThumbnail();
   thumbnaild.setFrameId(b->cur_frame_data.frame_id);
   thumbnaild.setTimestampEof(b->cur_frame_data.timestamp_eof);
-  thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len));
+  thumbnaild.setThumbnail(thumbnail);
 
   pm->send("thumbnail", msg);
-  free(thumbnail_buffer);
 }
 
 float set_exposure_target(const CameraBuf *b, int x_start, int x_end, int x_skip, int y_start, int y_end, int y_skip) {
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index dd46eb550..7b510058e 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -62,6 +62,7 @@ typedef struct LogCameraInfo {
   bool has_qcamera;
   bool trigger_rotate;
   bool enable;
+  bool record;
 } LogCameraInfo;
 
 typedef struct FrameMetadata {
diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc
new file mode 100644
index 000000000..a1060ef17
--- /dev/null
+++ b/selfdrive/camerad/cameras/camera_replay.cc
@@ -0,0 +1,125 @@
+#include "selfdrive/camerad/cameras/camera_replay.h"
+
+#include <cassert>
+#include <thread>
+
+#include "selfdrive/common/clutil.h"
+#include "selfdrive/common/util.h"
+
+extern ExitHandler do_exit;
+
+void camera_autoexposure(CameraState *s, float grey_frac) {}
+
+namespace {
+
+const char *BASE_URL = "https://commadataci.blob.core.windows.net/openpilotci/";
+
+const std::string road_camera_route = "0c94aa1e1296d7c6|2021-05-05--19-48-37";
+// const std::string driver_camera_route = "534ccd8a0950a00c|2021-06-08--12-15-37";
+
+std::string get_url(std::string route_name, const std::string &camera, int segment_num) {
+  std::replace(route_name.begin(), route_name.end(), '|', '/');
+  return util::string_format("%s%s/%d/%s.hevc", BASE_URL, route_name.c_str(), segment_num, camera.c_str());
+}
+
+void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) {
+  // TODO: cache url file
+  s->frame = new FrameReader();
+  if (!s->frame->load(url)) {
+    printf("failed to load stream from %s", url.c_str());
+    assert(0);
+  }
+
+  CameraInfo ci = {
+      .frame_width = s->frame->width,
+      .frame_height = s->frame->height,
+      .frame_stride = s->frame->width * 3,
+  };
+  s->ci = ci;
+  s->camera_num = camera_id;
+  s->fps = fps;
+  s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
+}
+
+void camera_close(CameraState *s) {
+  delete s->frame;
+}
+
+void run_camera(CameraState *s) {
+  uint32_t stream_frame_id = 0, frame_id = 0;
+  size_t buf_idx = 0;
+  std::unique_ptr<uint8_t[]> rgb_buf = std::make_unique<uint8_t[]>(s->frame->getRGBSize());
+  while (!do_exit) {
+    if (stream_frame_id == s->frame->getFrameCount()) {
+      // loop stream
+      stream_frame_id = 0;
+    }
+    if (s->frame->get(stream_frame_id++, rgb_buf.get(), nullptr)) {
+      s->buf.camera_bufs_metadata[buf_idx] = {.frame_id = frame_id};
+      auto &buf = s->buf.camera_bufs[buf_idx];
+      CL_CHECK(clEnqueueWriteBuffer(buf.copy_q, buf.buf_cl, CL_TRUE, 0, s->frame->getRGBSize(), rgb_buf.get(), 0, NULL, NULL));
+      s->buf.queue(buf_idx);
+      ++frame_id;
+      buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
+    }
+    util::sleep_for(1000 / s->fps);
+  }
+}
+
+void road_camera_thread(CameraState *s) {
+  set_thread_name("replay_road_camera_thread");
+  run_camera(s);
+}
+
+// void driver_camera_thread(CameraState *s) {
+//   set_thread_name("replay_driver_camera_thread");
+//   run_camera(s);
+// }
+
+void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
+  const CameraBuf *b = &c->buf;
+  MessageBuilder msg;
+  auto framed = msg.initEvent().initRoadCameraState();
+  fill_frame_data(framed, b->cur_frame_data);
+  framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len));
+  framed.setTransform(b->yuv_transform.v);
+  s->pm->send("roadCameraState", msg);
+}
+
+// void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) {
+//   MessageBuilder msg;
+//   auto framed = msg.initEvent().initDriverCameraState();
+//   framed.setFrameType(cereal::FrameData::FrameType::FRONT);
+//   fill_frame_data(framed, c->buf.cur_frame_data);
+//   s->pm->send("driverCameraState", msg);
+// }
+
+}  // namespace
+
+void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
+  camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
+              VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK, get_url(road_camera_route, "fcamera", 0));
+  // camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
+  //             VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT, get_url(driver_camera_route, "dcamera", 0));
+  s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
+}
+
+void cameras_open(MultiCameraState *s) {}
+
+void cameras_close(MultiCameraState *s) {
+  camera_close(&s->road_cam);
+  camera_close(&s->driver_cam);
+  delete s->pm;
+}
+
+void cameras_run(MultiCameraState *s) {
+  std::vector<std::thread> threads;
+  threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
+  // threads.push_back(start_process_thread(s, &s->driver_cam, process_driver_camera));
+  // threads.push_back(std::thread(driver_camera_thread, &s->driver_cam));
+  road_camera_thread(&s->road_cam);
+
+  for (auto &t : threads) t.join();
+
+  cameras_close(s);
+}
diff --git a/selfdrive/camerad/cameras/camera_replay.h b/selfdrive/camerad/cameras/camera_replay.h
new file mode 100644
index 000000000..7c41af0ab
--- /dev/null
+++ b/selfdrive/camerad/cameras/camera_replay.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include "selfdrive/camerad/cameras/camera_common.h"
+#include "selfdrive/ui/replay/framereader.h"
+
+#define FRAME_BUF_COUNT 16
+
+typedef struct CameraState {
+  int camera_num;
+  CameraInfo ci;
+
+  int fps;
+  float digital_gain = 0;
+
+  CameraBuf buf;
+  FrameReader *frame = nullptr;
+} CameraState;
+
+typedef struct MultiCameraState {
+  CameraState road_cam;
+  CameraState driver_cam;
+
+  SubMaster *sm = nullptr;
+  PubMaster *pm = nullptr;
+} MultiCameraState;
diff --git a/selfdrive/camerad/main.cc b/selfdrive/camerad/main.cc
index b4cf162d2..4d9312688 100644
--- a/selfdrive/camerad/main.cc
+++ b/selfdrive/camerad/main.cc
@@ -22,7 +22,7 @@
 #elif WEBCAM
 #include "selfdrive/camerad/cameras/camera_webcam.h"
 #else
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
+#include "selfdrive/camerad/cameras/camera_replay.h"
 #endif
 
 ExitHandler do_exit;
@@ -44,12 +44,11 @@ void party(cl_device_id device_id, cl_context context) {
 #endif
 
 int main(int argc, char *argv[]) {
-  set_realtime_priority(53);
-  if (Hardware::EON()) {
-    set_core_affinity(2);
-  } else if (Hardware::TICI()) {
-    set_core_affinity(6);
-  }
+  int ret;
+  ret = set_realtime_priority(53);
+  assert(ret == 0);
+  ret = set_core_affinity({Hardware::EON() ? 2 : 6});
+  assert(ret == 0 || Params().getBool("IsOffroad")); // failure ok while offroad due to offlining cores
 
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
 
diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py
index b7dd300da..787d62837 100755
--- a/selfdrive/camerad/snapshot/snapshot.py
+++ b/selfdrive/camerad/snapshot/snapshot.py
@@ -88,20 +88,20 @@ def snapshot():
 
   os.environ["SEND_ROAD"] = "1"
   os.environ["SEND_WIDE_ROAD"] = "1"
-
   if front_camera_allowed:
     os.environ["SEND_DRIVER"] = "1"
 
-  managed_processes['camerad'].start()
-  frame = "wideRoadCameraState" if TICI else "roadCameraState"
-  front_frame = "driverCameraState" if front_camera_allowed else None
-  focus_perc_threshold = 0. if TICI else 10 / 12.
+  try:
+    managed_processes['camerad'].start()
+    frame = "wideRoadCameraState" if TICI else "roadCameraState"
+    front_frame = "driverCameraState" if front_camera_allowed else None
+    focus_perc_threshold = 0. if TICI else 10 / 12.
 
-  rear, front = get_snapshots(frame, front_frame, focus_perc_threshold)
-  managed_processes['camerad'].stop()
-
-  params.put_bool("IsTakingSnapshot", False)
-  set_offroad_alert("Offroad_IsTakingSnapshot", False)
+    rear, front = get_snapshots(frame, front_frame, focus_perc_threshold)
+  finally:
+    managed_processes['camerad'].stop()
+    params.put_bool("IsTakingSnapshot", False)
+    set_offroad_alert("Offroad_IsTakingSnapshot", False)
 
   if not front_camera_allowed:
     front = None
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index 9cd13a58d..ddb20fe54 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -1,4 +1,5 @@
 # functions common among cars
+from cereal import car
 from common.numpy_fast import clip
 
 # kg of standard extra cargo to count for drive, gas, etc...
@@ -121,3 +122,11 @@ def create_gas_command(packer, gas_amount, idx):
 
 def make_can_msg(addr, dat, bus):
   return [addr, 0, dat, bus]
+
+
+def get_safety_config(safety_model, safety_param = None):
+  ret = car.CarParams.SafetyConfig.new_message()
+  ret.safetyModel = safety_model
+  if safety_param is not None:
+    ret.safetyParam = safety_param
+  return ret
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index c0c4f4bf0..f2e1b37e3 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -13,7 +13,7 @@ from cereal import car
 EventName = car.CarEvent.EventName
 
 
-def get_startup_event(car_recognized, controller_available, fuzzy_fingerprint, fw_seen):
+def get_startup_event(car_recognized, controller_available, fw_seen):
   if comma_remote and tested_branch:
     event = EventName.startup
   else:
@@ -26,8 +26,6 @@ def get_startup_event(car_recognized, controller_available, fuzzy_fingerprint, f
       event = EventName.startupNoFw
   elif car_recognized and not controller_available:
     event = EventName.startupNoControl
-  elif car_recognized and fuzzy_fingerprint:
-    event = EventName.startupFuzzyFingerprint
   return event
 
 
diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py
index d0f4ace97..1822d76a1 100755
--- a/selfdrive/car/chrysler/interface.py
+++ b/selfdrive/car/chrysler/interface.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.car.chrysler.values import CAR
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -10,10 +10,7 @@ class CarInterface(CarInterfaceBase):
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "chrysler"
-    ret.safetyModel = car.CarParams.SafetyModel.chrysler
-
-    # Chrysler port is a community feature, since we don't own one to test
-    ret.communityFeature = True
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.chrysler)]
 
     # Speed conversion:              20, 45 mph
     ret.wheelbase = 3.089  # in meters for Pacifica Hybrid 2017
diff --git a/selfdrive/car/disable_ecu.py b/selfdrive/car/disable_ecu.py
new file mode 100644
index 000000000..3a06cc06f
--- /dev/null
+++ b/selfdrive/car/disable_ecu.py
@@ -0,0 +1,34 @@
+from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
+from selfdrive.swaglog import cloudlog
+
+EXT_DIAG_REQUEST = b'\x10\x03'
+EXT_DIAG_RESPONSE = b'\x50\x03'
+
+COM_CONT_RESPONSE = b''
+
+def disable_ecu(logcan, sendcan, bus=0, addr=0x7d0, com_cont_req=b'\x28\x83\x01', timeout=0.1, retry=10, debug=False):
+  """Silence an ECU by disabling sending and receiving messages using UDS 0x28.
+  The ECU will stay silent as long as openpilot keeps sending Tester Present.
+
+  This is used to disable the radar in some cars. Openpilot will emulate the radar.
+  WARNING: THIS DISABLES AEB!"""
+  cloudlog.warning(f"ecu disable {hex(addr)} ...")
+
+  for i in range(retry):
+    try:
+      query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
+
+      for _, _ in query.get_data(timeout).items():
+        cloudlog.warning("communication control disable tx/rx ...")
+
+        query = IsoTpParallelQuery(sendcan, logcan, bus, [addr], [com_cont_req], [COM_CONT_RESPONSE], debug=debug)
+        query.get_data(0)
+
+        cloudlog.warning("ecu disabled")
+        return True
+    except Exception:
+      cloudlog.exception("ecu disable exception")
+
+    print(f"ecu disable retry ({i+1}) ...")
+  cloudlog.warning("ecu disable failed")
+  return False
diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py
index f62f89f65..8c0b1e1fa 100755
--- a/selfdrive/car/ford/interface.py
+++ b/selfdrive/car/ford/interface.py
@@ -2,7 +2,7 @@
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.car.ford.values import MAX_ANGLE
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -11,7 +11,7 @@ class CarInterface(CarInterfaceBase):
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "ford"
-    ret.safetyModel = car.CarParams.SafetyModel.ford
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)]
     ret.dashcamOnly = True
 
     ret.wheelbase = 2.85
diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py
index 4cb874d2d..2c58b8e3a 100755
--- a/selfdrive/car/fw_versions.py
+++ b/selfdrive/car/fw_versions.py
@@ -42,15 +42,12 @@ UDS_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
   p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION)
 
 
-HYUNDAI_VERSION_REQUEST_SHORT = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
-  p16(0xf1a0)  # 4 Byte version number
 HYUNDAI_VERSION_REQUEST_LONG = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
   p16(0xf100)  # Long description
 HYUNDAI_VERSION_REQUEST_MULTI = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER]) + \
   p16(uds.DATA_IDENTIFIER_TYPE.VEHICLE_MANUFACTURER_SPARE_PART_NUMBER) + \
   p16(uds.DATA_IDENTIFIER_TYPE.APPLICATION_SOFTWARE_IDENTIFICATION) + \
-  p16(0xf100) + \
-  p16(0xf1a0)
+  p16(0xf100) 
 HYUNDAI_VERSION_RESPONSE = bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40])
 
 
@@ -77,12 +74,6 @@ MAZDA_VERSION_RESPONSE =  bytes([uds.SERVICE_TYPE.READ_DATA_BY_IDENTIFIER + 0x40
 # brand, request, response, response offset
 REQUESTS = [
   # Hyundai
-  (
-    "hyundai",
-    [HYUNDAI_VERSION_REQUEST_SHORT],
-    [HYUNDAI_VERSION_RESPONSE],
-    DEFAULT_RX_OFFSET,
-  ),
   (
     "hyundai",
     [HYUNDAI_VERSION_REQUEST_LONG],
diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py
index 65030bfdb..53692c25f 100755
--- a/selfdrive/car/gm/interface.py
+++ b/selfdrive/car/gm/interface.py
@@ -1,9 +1,10 @@
 #!/usr/bin/env python3
 from cereal import car
+from math import fabs
 from selfdrive.config import Conversions as CV
 from selfdrive.car.gm.values import CAR, CruiseButtons, \
                                     AccState, CarControllerParams
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -15,17 +16,28 @@ class CarInterface(CarInterfaceBase):
     params = CarControllerParams()
     return params.ACCEL_MIN, params.ACCEL_MAX
 
+  # Volt determined by iteratively plotting and minimizing error for f(angle, speed) = steer.
+  @staticmethod
+  def get_steer_feedforward_volt(desired_angle, v_ego):
+    # maps [-inf,inf] to [-1,1]: sigmoid(34.4 deg) = sigmoid(1) = 0.5
+    # 1 / 0.02904609 = 34.4 deg ~= 36 deg ~= 1/10 circle? Arbitrary?
+    desired_angle *= 0.02904609
+    sigmoid = desired_angle / (1 + fabs(desired_angle))
+    return 0.10006696 * sigmoid * (v_ego + 3.12485927)
+
+  def get_steer_feedforward_function(self):
+    if self.CP.carFingerprint in [CAR.VOLT]:
+      return self.get_steer_feedforward_volt
+    else:
+      return CarInterfaceBase.get_steer_feedforward_default
+
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "gm"
-    ret.safetyModel = car.CarParams.SafetyModel.gm
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.gm)]
     ret.pcmCruise = False  # stock cruise control is kept off
 
-    # GM port is a community feature
-    # TODO: make a port that uses a car harness and it only intercepts the camera
-    ret.communityFeature = True
-
     # Presence of a camera on the object bus is ok.
     # Have to go to read_only if ASCM is online (ACC-enabled cars),
     # or camera is on powertrain bus (LKA cars without ACC).
@@ -45,9 +57,17 @@ class CarInterface(CarInterfaceBase):
       ret.minEnableSpeed = 18 * CV.MPH_TO_MS
       ret.mass = 1607. + STD_CARGO_KG
       ret.wheelbase = 2.69
-      ret.steerRatio = 15.7
+      ret.steerRatio = 17.7  # Stock 15.7, LiveParameters
+      tire_stiffness_factor = 0.469 # Stock Michelin Energy Saver A/S, LiveParameters
       ret.steerRatioRear = 0.
-      ret.centerToFront = ret.wheelbase * 0.4  # wild guess
+      ret.centerToFront = ret.wheelbase * 0.45 # Volt Gen 1, TODO corner weigh
+
+      ret.lateralTuning.pid.kpBP = [0., 40.]
+      ret.lateralTuning.pid.kpV = [0., 0.17]
+      ret.lateralTuning.pid.kiBP = [0.]
+      ret.lateralTuning.pid.kiV = [0.]
+      ret.lateralTuning.pid.kf = 1. # get_steer_feedforward_volt()
+      ret.steerActuatorDelay = 0.2
 
     elif candidate == CAR.MALIBU:
       # supports stop and go, but initial engage must be above 18mph (which include conservatism)
@@ -105,8 +125,6 @@ class CarInterface(CarInterfaceBase):
     ret.longitudinalTuning.kiBP = [0.]
     ret.longitudinalTuning.kiV = [0.36]
 
-    ret.startAccel = 0.8
-
     ret.steerLimitTimer = 0.4
     ret.radarTimeStep = 0.0667  # GM radar runs at 15Hz instead of standard 20Hz
 
diff --git a/selfdrive/car/gm/values.py b/selfdrive/car/gm/values.py
index 15860e6b7..cb4a68a69 100644
--- a/selfdrive/car/gm/values.py
+++ b/selfdrive/car/gm/values.py
@@ -5,41 +5,40 @@ from selfdrive.car import dbc_dict
 Ecu = car.CarParams.Ecu
 
 class CarControllerParams():
-  def __init__(self):
-    self.STEER_MAX = 300
-    self.STEER_STEP = 2              # how often we update the steer cmd
-    self.STEER_DELTA_UP = 7          # ~0.75s time to peak torque (255/50hz/0.75s)
-    self.STEER_DELTA_DOWN = 17       # ~0.3s from peak torque to zero
-    self.MIN_STEER_SPEED = 3.
-    self.STEER_DRIVER_ALLOWANCE = 50   # allowed driver torque before start limiting
-    self.STEER_DRIVER_MULTIPLIER = 4   # weight driver torque heavily
-    self.STEER_DRIVER_FACTOR = 100     # from dbc
-    self.NEAR_STOP_BRAKE_PHASE = 0.5  # m/s, more aggressive braking near full stop
+  STEER_MAX = 300  # Safety limit, not LKA max. Trucks use 600.
+  STEER_STEP = 2  # control frames per command
+  STEER_DELTA_UP = 7
+  STEER_DELTA_DOWN = 17
+  MIN_STEER_SPEED = 3.  # m/s
+  STEER_DRIVER_ALLOWANCE = 50
+  STEER_DRIVER_MULTIPLIER = 4
+  STEER_DRIVER_FACTOR = 100
+  NEAR_STOP_BRAKE_PHASE = 0.5  # m/s
 
-    # Takes case of "Service Adaptive Cruise" and "Service Front Camera"
-    # dashboard messages.
-    self.ADAS_KEEPALIVE_STEP = 100
-    self.CAMERA_KEEPALIVE_STEP = 100
+  # Heartbeat for dash "Service Adaptive Cruise" and "Service Front Camera"
+  ADAS_KEEPALIVE_STEP = 100
+  CAMERA_KEEPALIVE_STEP = 100
 
-    # pedal lookups, only for Volt
-    MAX_GAS = 3072              # Only a safety limit
-    ZERO_GAS = 2048
-    MAX_BRAKE = 350             # Should be around 3.5m/s^2, including regen
+  # Volt gasbrake lookups
+  MAX_GAS = 3072 # Safety limit, not ACC max. Stock ACC >4096 from standstill.
+  ZERO_GAS = 2048 # Coasting
+  MAX_BRAKE = 350 # ~ -3.5 m/s^2 with regen
 
-    self.ACCEL_MAX = 2.0 # m/s^2
+  # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
+  # perform the closed loop control, and might need some
+  # to apply some more braking if we're on a downhill slope.
+  # Our controller should still keep the 2 second average above
+  # -3.5 m/s^2 as per planner limits
+  ACCEL_MAX = 2. # m/s^2
+  ACCEL_MIN = -4. # m/s^2
 
-    # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
-    # perform the closed loop control, and might need some
-    # to apply some more braking if we're on a downhill slope.
-    # Our controller should still keep the 2 second average above
-    # -3.5 m/s^2 as per planner limits
-    self.ACCEL_MIN = -4.0 # m/s^2
+  MAX_ACC_REGEN = 1404  # Max ACC regen is slightly less than max paddle regen
+  GAS_LOOKUP_BP = [-1., 0., ACCEL_MAX]
+  GAS_LOOKUP_V = [MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
+  BRAKE_LOOKUP_BP = [ACCEL_MIN, -1.]
+  BRAKE_LOOKUP_V = [MAX_BRAKE, 0.]
 
-    self.MAX_ACC_REGEN = 1404  # ACC Regen braking is slightly less powerful than max regen paddle
-    self.GAS_LOOKUP_BP = [-1.0, 0., self.ACCEL_MAX]
-    self.GAS_LOOKUP_V = [self.MAX_ACC_REGEN, ZERO_GAS, MAX_GAS]
-    self.BRAKE_LOOKUP_BP = [self.ACCEL_MIN, -1.0]
-    self.BRAKE_LOOKUP_V = [MAX_BRAKE, 0]
+STEER_THRESHOLD = 1.0
 
 class CAR:
   HOLDEN_ASTRA = "HOLDEN ASTRA RS-V BK 2017"
@@ -109,8 +108,6 @@ FINGERPRINTS = {
   }],
 }
 
-STEER_THRESHOLD = 1.0
-
 DBC = {
   CAR.HOLDEN_ASTRA: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
   CAR.VOLT: dbc_dict('gm_global_a_powertrain', 'gm_global_a_object', chassis_dbc='gm_global_a_chassis'),
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index b57bd79ac..088e42476 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -107,8 +107,7 @@ class CarController():
 
     self.params = CarControllerParams(CP)
 
-  def update(self, enabled, CS, frame, actuators,
-             pcm_speed, pcm_override, pcm_cancel_cmd, pcm_accel,
+  def update(self, enabled, active, CS, frame, actuators, pcm_cancel_cmd,
              hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
 
     P = self.params
@@ -123,15 +122,6 @@ class CarController():
     # *** apply brake hysteresis ***
     pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
 
-    # *** no output if not enabled ***
-    if not enabled and CS.out.cruiseState.enabled:
-      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
-      pcm_cancel_cmd = True
-
-    # Never send cancel command if we never enter cruise state (no cruise if pedal)
-    # Cancel cmd causes brakes to release at a standstill causing grinding
-    pcm_cancel_cmd = pcm_cancel_cmd and CS.CP.pcmCruise
-
     # *** rate limit after the enable check ***
     self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
 
@@ -175,9 +165,6 @@ class CarController():
     stopping = actuators.longControlState == LongCtrlState.stopping
     starting = actuators.longControlState == LongCtrlState.starting
 
-    # Prevent rolling backwards
-    accel = -4.0 if stopping else accel
-
     # wind brake from air resistance decel at high speed
     wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
     # all of this is only relevant for HONDA NIDEC
@@ -189,21 +176,24 @@ class CarController():
                       0.5]
     # The Honda ODYSSEY seems to have different PCM_ACCEL
     # msgs, is it other cars too?
-    if CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
+    if CS.CP.enableGasInterceptor:
+      pcm_speed = 0.0
+      pcm_accel = int(0.0)
+    elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
       pcm_speed_V = [0.0,
                      clip(CS.out.vEgo - 3.0, 0.0, 100.0),
                      clip(CS.out.vEgo + 0.0, 0.0, 100.0),
                      clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
+      pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
       pcm_accel = int((1.0) * 0xc6)
     else:
       pcm_speed_V = [0.0,
                      clip(CS.out.vEgo - 2.0, 0.0, 100.0),
                      clip(CS.out.vEgo + 2.0, 0.0, 100.0),
                      clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
+      pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
       pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6)
 
-    pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
-
     if not CS.CP.openpilotLongitudinalControl:
       if (frame % 2) == 0:
         idx = frame // 2
@@ -221,13 +211,16 @@ class CarController():
         ts = frame * DT_CTRL
 
         if CS.CP.carFingerprint in HONDA_BOSCH:
+          accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
           bosch_gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
-          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
+          can_sends.extend(hondacan.create_acc_commands(self.packer, enabled, active, accel, bosch_gas, idx, stopping, starting, CS.CP.carFingerprint))
 
         else:
           apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
-          apply_brake = int(clip(apply_brake * P.BRAKE_MAX, 0, P.BRAKE_MAX - 1))
+          apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
           pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
+
+          pcm_override = True
           can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
             pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
           self.apply_brake_last = apply_brake
@@ -237,7 +230,12 @@ class CarController():
             gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
             # send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
             # This prevents unexpected pedal range rescaling
-            apply_gas = clip(gas_mult * gas, 0., 1.)
+            # Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
+            # when you do enable.
+            if enabled:
+              apply_gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
+            else:
+              apply_gas = 0.0
             can_sends.append(create_gas_command(self.packer, apply_gas, idx))
 
     hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index e9766e073..c45573c72 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -10,19 +10,7 @@ from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR,
 TransmissionType = car.CarParams.TransmissionType
 
 
-def calc_cruise_offset(offset, speed):
-  # heuristic formula so that speed is controlled to ~ 0.3m/s below pid_speed
-  # constraints to solve for _K0, _K1, _K2 are:
-  # - speed = 0m/s, out = -0.3
-  # - speed = 34m/s, offset = 20, out = -0.25
-  # - speed = 34m/s, offset = -2.5, out = -1.8
-  _K0 = -0.3
-  _K1 = -0.01879
-  _K2 = 0.01013
-  return min(_K0 + _K1 * speed + _K2 * speed * offset, 0.)
-
-
-def get_can_signals(CP, gearbox_msg="GEARBOX"):
+def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
   # this function generates lists for signal, messages and initial values
   signals = [
     ("XMISSION_SPEED", "ENGINE_DATA", 0),
@@ -50,6 +38,7 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
     ("PEDAL_GAS", "POWERTRAIN_DATA", 0),
     ("CRUISE_SETTING", "SCM_BUTTONS", 0),
     ("ACC_STATUS", "POWERTRAIN_DATA", 0),
+    ("MAIN_ON", main_on_sig_msg, 0),
   ]
 
   checks = [
@@ -90,13 +79,12 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
 
   if CP.carFingerprint in HONDA_BOSCH:
     signals += [
-      ("CAR_GAS", "GAS_PEDAL_2", 0),
-      ("MAIN_ON", "SCM_FEEDBACK", 0),
       ("EPB_STATE", "EPB_STATUS", 0),
+      ("IMPERIAL_UNIT", "CAR_SPEED", 1),
     ]
     checks += [
       ("EPB_STATUS", 50),
-      ("GAS_PEDAL_2", 100),
+      ("CAR_SPEED", 10),
     ]
 
     if not CP.openpilotLongitudinalControl:
@@ -123,7 +111,7 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
     signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
   elif CP.carFingerprint == CAR.ODYSSEY_CHN:
     signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
-  elif CP.carFingerprint == CAR.HRV:
+  elif CP.carFingerprint in (CAR.FREED, CAR.HRV):
     signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1),
                 ("WHEELS_MOVING", "STANDSTILL", 1)]
   else:
@@ -138,46 +126,14 @@ def get_can_signals(CP, gearbox_msg="GEARBOX"):
     ]
 
   if CP.carFingerprint == CAR.CIVIC:
-    signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
-                ("MAIN_ON", "SCM_FEEDBACK", 0),
-                ("IMPERIAL_UNIT", "HUD_SETTING", 0),
+    signals += [("IMPERIAL_UNIT", "HUD_SETTING", 0),
                 ("EPB_STATE", "EPB_STATUS", 0)]
     checks += [
       ("HUD_SETTING", 50),
       ("EPB_STATUS", 50),
-      ("GAS_PEDAL_2", 100),
     ]
-  elif CP.carFingerprint == CAR.ACURA_ILX:
-    signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
-                ("MAIN_ON", "SCM_BUTTONS", 0)]
-    checks += [
-      ("GAS_PEDAL_2", 100),
-    ]
-  elif CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.PILOT_2019, CAR.RIDGELINE):
-    signals += [("MAIN_ON", "SCM_BUTTONS", 0)]
-  elif CP.carFingerprint == CAR.FIT:
-    signals += [("CAR_GAS", "GAS_PEDAL_2", 0),
-                ("MAIN_ON", "SCM_BUTTONS", 0),
-                ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
-    checks += [
-      ("GAS_PEDAL_2", 100),
-    ]
-  elif CP.carFingerprint == CAR.HRV:
-    signals += [("MAIN_ON", "SCM_BUTTONS", 0),
-                ("BRAKE_HOLD_ACTIVE", "VSA_STATUS", 0)]
-  elif CP.carFingerprint == CAR.ODYSSEY:
-    signals += [("MAIN_ON", "SCM_FEEDBACK", 0),
-                ("EPB_STATE", "EPB_STATUS", 0)]
-    checks += [("EPB_STATUS", 50)]
-  elif CP.carFingerprint == CAR.PILOT:
-    signals += [("MAIN_ON", "SCM_BUTTONS", 0),
-                ("CAR_GAS", "GAS_PEDAL_2", 0)]
-    checks += [
-      ("GAS_PEDAL_2", 0),  # TODO: fix this freq, seems this signal isn't present at all on some models
-    ]
-  elif CP.carFingerprint == CAR.ODYSSEY_CHN:
-    signals += [("MAIN_ON", "SCM_BUTTONS", 0),
-                ("EPB_STATE", "EPB_STATUS", 0)]
+  elif CP.carFingerprint in (CAR.ODYSSEY, CAR.ODYSSEY_CHN):
+    signals += [("EPB_STATE", "EPB_STATUS", 0)]
     checks += [("EPB_STATUS", 50)]
 
   # add gas interceptor reading if we are using it
@@ -204,6 +160,11 @@ class CarState(CarStateBase):
     if CP.carFingerprint == CAR.ACCORD and CP.transmissionType == TransmissionType.cvt:
       self.gearbox_msg = "GEARBOX_15T"
 
+    self.main_on_sig_msg = "SCM_FEEDBACK"
+    if CP.carFingerprint in (CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV,
+                             CAR.ODYSSEY_CHN, CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
+      self.main_on_sig_msg = "SCM_BUTTONS"
+
     self.shifter_values = can_define.dv[self.gearbox_msg]["GEAR_SHIFTER"]
     self.steer_status_values = defaultdict(lambda: "UNKNOWN", can_define.dv["STEER_STATUS"]["STEER_STATUS"])
 
@@ -231,7 +192,7 @@ class CarState(CarStateBase):
     elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
       ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
       ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
-    elif self.CP.carFingerprint == CAR.HRV:
+    elif self.CP.carFingerprint in (CAR.FREED, CAR.HRV):
       ret.standstill = not cp.vl["STANDSTILL"]["WHEELS_MOVING"]
       ret.doorOpen = bool(cp.vl["SCM_BUTTONS"]["DRIVERS_DOOR_OPEN"])
     else:
@@ -275,34 +236,24 @@ class CarState(CarStateBase):
       250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
     self.brake_hold = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"]
 
-    if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
+    if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
                                   CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
       self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
-      main_on = cp.vl["SCM_FEEDBACK"]["MAIN_ON"]
-    elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
-      self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
-      main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
     else:
       self.park_brake = 0  # TODO
-      main_on = cp.vl["SCM_BUTTONS"]["MAIN_ON"]
 
     gear = int(cp.vl[self.gearbox_msg]["GEAR_SHIFTER"])
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None))
 
-    pedal_gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
-    # crv doesn't include cruise control
-    if self.CP.carFingerprint in (CAR.CRV, CAR.CRV_EU, CAR.HRV, CAR.ODYSSEY, CAR.ACURA_RDX, CAR.RIDGELINE, CAR.PILOT_2019, CAR.ODYSSEY_CHN):
-      ret.gas = pedal_gas / 256.
-    else:
-      ret.gas = cp.vl["GAS_PEDAL_2"]["CAR_GAS"] / 256.
+    ret.gas = cp.vl["POWERTRAIN_DATA"]["PEDAL_GAS"]
 
     # this is a hack for the interceptor. This is now only used in the simulation
     # TODO: Replace tests by toyota so this can go away
     if self.CP.enableGasInterceptor:
       user_gas = (cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS"] + cp.vl["GAS_SENSOR"]["INTERCEPTOR_GAS2"]) / 2.
-      ret.gasPressed = user_gas > 1e-5  # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change
+      ret.gasPressed = user_gas > 1e-5  # this works because interceptor reads < 0 when pedal position is 0. Once calibrated, this will change
     else:
-      ret.gasPressed = pedal_gas > 1e-5
+      ret.gasPressed = ret.gas > 1e-5
 
     ret.steeringTorque = cp.vl["STEER_STATUS"]["STEER_TORQUE_SENSOR"]
     ret.steeringTorqueEps = cp.vl["STEER_MOTOR_TORQUE"]["MOTOR_TORQUE"]
@@ -317,7 +268,6 @@ class CarState(CarStateBase):
         ret.cruiseState.speed = self.v_cruise_pcm_prev if cp.vl["ACC_HUD"]["CRUISE_SPEED"] > 160.0 else cp.vl["ACC_HUD"]["CRUISE_SPEED"] * CV.KPH_TO_MS
         self.v_cruise_pcm_prev = ret.cruiseState.speed
     else:
-      ret.cruiseState.speedOffset = calc_cruise_offset(cp.vl["CRUISE_PARAMS"]["CRUISE_SPEED_OFFSET"], ret.vEgo)
       ret.cruiseState.speed = cp.vl["CRUISE"]["CRUISE_SPEED_PCM"] * CV.KPH_TO_MS
 
     self.brake_switch = cp.vl["POWERTRAIN_DATA"]["BRAKE_SWITCH"] != 0
@@ -335,7 +285,7 @@ class CarState(CarStateBase):
 
     ret.brake = cp.vl["VSA_STATUS"]["USER_BRAKE"]
     ret.cruiseState.enabled = cp.vl["POWERTRAIN_DATA"]["ACC_STATUS"] != 0
-    ret.cruiseState.available = bool(main_on)
+    ret.cruiseState.available = bool(cp.vl[self.main_on_sig_msg]["MAIN_ON"])
 
     # Gets rid of Pedal Grinding noise when brake is pressed at slow speeds for some models
     if self.CP.carFingerprint in (CAR.PILOT, CAR.PILOT_2019, CAR.RIDGELINE):
@@ -343,7 +293,12 @@ class CarState(CarStateBase):
         ret.brakePressed = True
 
     # TODO: discover the CAN msg that has the imperial unit bit for all other cars
-    self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"] if self.CP.carFingerprint in (CAR.CIVIC) else False
+    if self.CP.carFingerprint in (CAR.CIVIC, ):
+      self.is_metric = not cp.vl["HUD_SETTING"]["IMPERIAL_UNIT"]
+    elif self.CP.carFingerprint in HONDA_BOSCH:
+      self.is_metric = not cp.vl["CAR_SPEED"]["IMPERIAL_UNIT"]
+    else:
+      self.is_metric = False
 
     if self.CP.carFingerprint in HONDA_BOSCH:
       ret.stockAeb = (not self.CP.openpilotLongitudinalControl) and bool(cp.vl["ACC_CONTROL"]["AEB_STATUS"] and cp.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5)
@@ -367,18 +322,16 @@ class CarState(CarStateBase):
     return ret
 
   def get_can_parser(self, CP):
-    signals, checks = get_can_signals(CP, self.gearbox_msg)
+    signals, checks = get_can_signals(CP, self.gearbox_msg, self.main_on_sig_msg)
     bus_pt = 1 if CP.carFingerprint in HONDA_BOSCH else 0
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, bus_pt)
 
   @staticmethod
   def get_cam_can_parser(CP):
     signals = []
-
-    # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering
-    checks = [(0xe4, 100)]
-    if CP.carFingerprint in [CAR.CRV, CAR.CRV_EU, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]:
-      checks = [(0x194, 100)]
+    checks = [
+      ("STEERING_CONTROL", 100),
+    ]
 
     if CP.carFingerprint not in HONDA_BOSCH:
       signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0),
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index d30f5b54f..5bcf59c25 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -1,7 +1,5 @@
-from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
-from selfdrive.car.honda.values import HONDA_BOSCH, CAR
+from selfdrive.car.honda.values import HONDA_BOSCH, CAR, CarControllerParams
 from selfdrive.config import Conversions as CV
-from selfdrive.swaglog import cloudlog
 
 # CAN bus layout with relay
 # 0 = ACC-CAN - radar side
@@ -9,13 +7,6 @@ from selfdrive.swaglog import cloudlog
 # 2 = ACC-CAN - camera side
 # 3 = F-CAN A - OBDII port
 
-RADAR_ADDR = 0x18DAB0F1
-EXT_DIAG_REQUEST = b'\x10\x03'
-EXT_DIAG_RESPONSE = b'\x50\x03'
-COM_CONT_REQUEST = b'\x28\x83\x03'
-COM_CONT_RESPONSE = b''
-
-
 def get_pt_bus(car_fingerprint):
   return 1 if car_fingerprint in HONDA_BOSCH else 0
 
@@ -27,30 +18,6 @@ def get_lkas_cmd_bus(car_fingerprint, radar_disabled=False):
   # normally steering commands are sent to radar, which forwards them to powertrain bus
   return 0
 
-
-def disable_radar(logcan, sendcan, bus=1, timeout=0.1, debug=False):
-  """Silence the radar by disabling sending and receiving messages using UDS 0x28.
-  The radar will stay silent as long as openpilot keeps sending Tester Present.
-  Openpilot will emulate the radar. WARNING: THIS DISABLES AEB!"""
-  cloudlog.warning(f"radar disable {hex(RADAR_ADDR)} ...")
-
-  try:
-    query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [EXT_DIAG_REQUEST], [EXT_DIAG_RESPONSE], debug=debug)
-
-    for _, _ in query.get_data(timeout).items():
-      cloudlog.warning("radar communication control disable tx/rx ...")
-
-      query = IsoTpParallelQuery(sendcan, logcan, bus, [RADAR_ADDR], [COM_CONT_REQUEST], [COM_CONT_RESPONSE], debug=debug)
-      query.get_data(0)
-
-      cloudlog.warning("radar disabled")
-      return
-
-  except Exception:
-    cloudlog.exception("radar disable exception")
-  cloudlog.warning("radar disable failed")
-
-
 def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, stock_brake):
   # TODO: do we loose pressure if we keep pump off for long?
   brakelights = apply_brake > 0
@@ -76,23 +43,23 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
   return packer.make_can_msg("BRAKE_COMMAND", bus, values, idx)
 
 
-def create_acc_commands(packer, enabled, accel, gas, idx, stopping, starting, car_fingerprint):
+def create_acc_commands(packer, enabled, active, accel, gas, idx, stopping, starting, car_fingerprint):
   commands = []
   bus = get_pt_bus(car_fingerprint)
+  min_gas_accel = CarControllerParams.BOSCH_GAS_LOOKUP_BP[0]
 
   control_on = 5 if enabled else 0
-  # no gas = -30000
-  gas_command = gas if enabled and gas > 0 else -30000
-  accel_command = accel if enabled else 0
-  braking = 1 if enabled and accel < 0 else 0
-  standstill = 1 if enabled and stopping else 0
-  standstill_release = 1 if enabled and starting else 0
+  gas_command = gas if active and accel > min_gas_accel else -30000
+  accel_command = accel if active else 0
+  braking = 1 if active and accel < min_gas_accel else 0
+  standstill = 1 if active and stopping else 0
+  standstill_release = 1 if active and starting else 0
 
   acc_control_values = {
     # setting CONTROL_ON causes car to set POWERTRAIN_DATA->ACC_STATUS = 1
     "CONTROL_ON": control_on,
-    "GAS_COMMAND": gas_command, # used for gas
-    "ACCEL_COMMAND": accel_command, # used for brakes
+    "GAS_COMMAND": gas_command,  # used for gas
+    "ACCEL_COMMAND": accel_command,  # used for brakes
     "BRAKE_LIGHTS": braking,
     "BRAKE_REQUEST": braking,
     "STANDSTILL": standstill,
@@ -148,6 +115,7 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
         'ACC_ON': hud.car != 0,
         'SET_TO_X1': 1,
         'IMPERIAL_UNIT': int(not is_metric),
+        'FCM_OFF': 1,
       }
     else:
       acc_hud_values = {
@@ -178,7 +146,8 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx,
 
   if radar_disabled and car_fingerprint in HONDA_BOSCH:
     radar_hud_values = {
-      'SET_TO_1' : 0x01,
+      'CMBS_OFF': 0x01,
+      'SET_TO_1': 0x01,
     }
     commands.append(packer.make_can_msg('RADAR_HUD', bus_pt, radar_hud_values, idx))
 
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index f99e6f75b..adeec6bd4 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -4,9 +4,9 @@ from panda import Panda
 from common.numpy_fast import interp
 from common.params import Params
 from selfdrive.car.honda.values import CarControllerParams, CruiseButtons, CAR, HONDA_BOSCH, HONDA_BOSCH_ALT_BRAKE_SIGNAL
-from selfdrive.car.honda.hondacan import disable_radar
-from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, CivicParams, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
+from selfdrive.car.disable_ecu import disable_ecu
 from selfdrive.config import Conversions as CV
 
 
@@ -27,45 +27,13 @@ class CarInterface(CarInterfaceBase):
       ACCEL_MAX_BP = [cruise_speed - 2., cruise_speed - .2]
       return CarControllerParams.NIDEC_ACCEL_MIN, interp(current_speed, ACCEL_MAX_BP, ACCEL_MAX_VALS)
 
-  @staticmethod
-  def calc_accel_override(a_ego, a_target, v_ego, v_target):
-
-    # normalized max accel. Allowing max accel at low speed causes speed overshoots
-    max_accel_bp = [10, 20]    # m/s
-    max_accel_v = [0.714, 1.0]  # unit of max accel
-    max_accel = interp(v_ego, max_accel_bp, max_accel_v)
-
-    # limit the pcm accel cmd if:
-    # - v_ego exceeds v_target, or
-    # - a_ego exceeds a_target and v_ego is close to v_target
-
-    eA = a_ego - a_target
-    valuesA = [1.0, 0.1]
-    bpA = [0.3, 1.1]
-
-    eV = v_ego - v_target
-    valuesV = [1.0, 0.1]
-    bpV = [0.0, 0.5]
-
-    valuesRangeV = [1., 0.]
-    bpRangeV = [-1., 0.]
-
-    # only limit if v_ego is close to v_target
-    speedLimiter = interp(eV, bpV, valuesV)
-    accelLimiter = max(interp(eA, bpA, valuesA), interp(eV, bpRangeV, valuesRangeV))
-
-    # accelOverride is more or less the max throttle allowed to pcm: usually set to a constant
-    # unless aTargetMax is very high and then we scale with it; this help in quicker restart
-
-    return float(max(max_accel, a_target / CarControllerParams.NIDEC_ACCEL_MAX)) * min(speedLimiter, accelLimiter)
-
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "honda"
 
     if candidate in HONDA_BOSCH:
-      ret.safetyModel = car.CarParams.SafetyModel.hondaBoschHarness
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaBoschHarness)]
       ret.radarOffCan = True
 
       # Disable the radar and let openpilot control longitudinal
@@ -73,9 +41,8 @@ class CarInterface(CarInterfaceBase):
       ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar")
 
       ret.pcmCruise = not ret.openpilotLongitudinalControl
-      ret.communityFeature = ret.openpilotLongitudinalControl
     else:
-      ret.safetyModel = car.CarParams.SafetyModel.hondaNidec
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hondaNidec)]
       ret.enableGasInterceptor = 0x201 in fingerprint[0]
       ret.openpilotLongitudinalControl = True
 
@@ -97,11 +64,16 @@ class CarInterface(CarInterfaceBase):
     ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
     ret.lateralTuning.pid.kf = 0.00006  # conservative feed-forward
 
-    # default longitudinal tuning for all hondas
-    ret.longitudinalTuning.kpBP = [0., 5., 35.]
-    ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
-    ret.longitudinalTuning.kiBP = [0., 35.]
-    ret.longitudinalTuning.kiV = [0.18, 0.12]
+    if candidate in HONDA_BOSCH:
+      ret.longitudinalTuning.kpV = [0.25]
+      ret.longitudinalTuning.kiV = [0.05]
+      ret.longitudinalActuatorDelayUpperBound = 0.5 # s
+    else:
+      # default longitudinal tuning for all hondas
+      ret.longitudinalTuning.kpBP = [0., 5., 35.]
+      ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5]
+      ret.longitudinalTuning.kiBP = [0., 35.]
+      ret.longitudinalTuning.kiV = [0.18, 0.12]
 
     eps_modified = False
     for fw in car_fw:
@@ -209,6 +181,17 @@ class CarInterface(CarInterfaceBase):
       tire_stiffness_factor = 0.75
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
 
+    elif candidate == CAR.FREED:
+      stop_and_go = False
+      ret.mass = 3086. * CV.LB_TO_KG + STD_CARGO_KG
+      ret.wheelbase = 2.74
+      # the remaining parameters were copied from FIT
+      ret.centerToFront = ret.wheelbase * 0.39
+      ret.steerRatio = 13.06
+      ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
+      tire_stiffness_factor = 0.75
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]]
+
     elif candidate == CAR.HRV:
       stop_and_go = False
       ret.mass = 3125 * CV.LB_TO_KG + STD_CARGO_KG
@@ -304,10 +287,10 @@ class CarInterface(CarInterfaceBase):
 
     # These cars use alternate user brake msg (0x1BE)
     if candidate in HONDA_BOSCH_ALT_BRAKE_SIGNAL:
-      ret.safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_ALT_BRAKE
 
     if ret.openpilotLongitudinalControl and candidate in HONDA_BOSCH:
-      ret.safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HONDA_BOSCH_LONG
 
     # min speed to enable ACC. if car can do stop and go, then set enabling speed
     # to a negative value, so it won't matter. Otherwise, add 0.5 mph margin to not
@@ -323,8 +306,6 @@ class CarInterface(CarInterfaceBase):
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
 
-    ret.startAccel = 0.5
-
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.5
     ret.steerLimitTimer = 0.8
@@ -334,7 +315,7 @@ class CarInterface(CarInterfaceBase):
   @staticmethod
   def init(CP, logcan, sendcan):
     if CP.carFingerprint in HONDA_BOSCH and CP.openpilotLongitudinalControl:
-      disable_radar(logcan, sendcan)
+      disable_ecu(logcan, sendcan, bus=1, addr=0x18DAB0F1, com_cont_req=b'\x28\x83\x03')
 
   # returns a car.CarState
   def update(self, c, can_strings):
@@ -437,12 +418,9 @@ class CarInterface(CarInterfaceBase):
     else:
       hud_v_cruise = 255
 
-    can_sends = self.CC.update(c.enabled, self.CS, self.frame,
+    can_sends = self.CC.update(c.enabled, c.active, self.CS, self.frame,
                                c.actuators,
-                               c.cruiseControl.speedOverride,
-                               c.cruiseControl.override,
                                c.cruiseControl.cancel,
-                               c.cruiseControl.accelOverride,
                                hud_v_cruise,
                                c.hudControl.lanesVisible,
                                hud_show_car=c.hudControl.leadVisible,
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 8e0b59d36..26a6d2ce3 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -4,20 +4,31 @@ from selfdrive.car import dbc_dict
 Ecu = car.CarParams.Ecu
 VisualAlert = car.CarControl.HUDControl.VisualAlert
 
+
 class CarControllerParams():
   # Allow small margin below -3.5 m/s^2 from ISO 15622:2018 since we
   # perform the closed loop control, and might need some
   # to apply some more braking if we're on a downhill slope.
   # Our controller should still keep the 2 second average above
   # -3.5 m/s^2 as per planner limits
-  NIDEC_ACCEL_MIN = -4.0 # m/s^2
-  NIDEC_ACCEL_MAX = 1.6 # m/s^2, lower than 2.0 m/s^2 for tuning reasons
+  NIDEC_ACCEL_MIN = -4.0  # m/s^2
+  NIDEC_ACCEL_MAX = 1.6  # m/s^2, lower than 2.0 m/s^2 for tuning reasons
 
-  BOSCH_ACCEL_MIN = -3.5 # m/s^2
-  BOSCH_ACCEL_MAX = 2.0 # m/s^2
+  NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
+  NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
+
+  NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
+  NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
+
+  NIDEC_BRAKE_MAX = 1024 // 4
+
+  BOSCH_ACCEL_MIN = -3.5  # m/s^2
+  BOSCH_ACCEL_MAX = 2.0  # m/s^2
+
+  BOSCH_GAS_LOOKUP_BP = [-0.2, 2.0]  # 2m/s^2
+  BOSCH_GAS_LOOKUP_V = [0, 1600]
 
   def __init__(self, CP):
-    self.BRAKE_MAX = 1024//4
     self.STEER_MAX = CP.lateralParams.torqueBP[-1]
     # mirror of list (assuming first item is zero) for interp of signed request values
     assert(CP.lateralParams.torqueBP[0] == 0)
@@ -25,15 +36,6 @@ class CarControllerParams():
     self.STEER_LOOKUP_BP = [v * -1 for v in CP.lateralParams.torqueBP][1:][::-1] + list(CP.lateralParams.torqueBP)
     self.STEER_LOOKUP_V = [v * -1 for v in CP.lateralParams.torqueV][1:][::-1] + list(CP.lateralParams.torqueV)
 
-    self.NIDEC_ACCEL_LOOKUP_BP = [-1., 0., .6]
-    self.NIDEC_ACCEL_LOOKUP_V = [-4.8, 0., 2.0]
-
-    self.NIDEC_MAX_ACCEL_V = [0.5, 2.4, 1.4, 0.6]
-    self.NIDEC_MAX_ACCEL_BP = [0.0, 4.0, 10., 20.]
-
-    self.BOSCH_GAS_LOOKUP_BP = [0., 2.0]  # 2m/s^2
-    self.BOSCH_GAS_LOOKUP_V = [0, 2000]
-
 
 # Car button codes
 class CruiseButtons:
@@ -66,6 +68,7 @@ class CAR:
   CRV_EU = "HONDA CR-V EU 2016"
   CRV_HYBRID = "HONDA CR-V HYBRID 2019"
   FIT = "HONDA FIT 2018"
+  FREED = "HONDA FREED 2020"
   HRV = "HONDA HRV 2019"
   ODYSSEY = "HONDA ODYSSEY 2018"
   ODYSSEY_CHN = "HONDA ODYSSEY CHN 2019"
@@ -81,39 +84,9 @@ class CAR:
 DIAG_MSGS = {1600: 5, 1601: 8}
 
 FINGERPRINTS = {
-  CAR.ACCORDH: [{
-    148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 387: 8, 388: 8, 399: 7, 419: 8, 420: 8, 427: 3, 432: 7, 441: 5, 450: 8, 464: 8, 477: 8, 479: 8, 495: 8, 525: 8, 545: 6, 662: 4, 773: 7, 777: 8, 780: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 927: 8, 929: 8, 1302: 8, 1600: 5, 1601: 8, 1652: 8
-  }],
-  CAR.ACURA_RDX: [{
-    57: 3, 145: 8, 229: 4, 308: 5, 316: 8, 342: 6, 344: 8, 380: 8, 392: 6, 398: 3, 399: 6, 404: 4, 420: 8, 422: 8, 426: 8, 432: 7, 464: 8, 474: 5, 476: 4, 487: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 4, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 800: 8, 804: 8, 808: 8, 819: 7, 821: 5, 829: 5, 882: 2, 884: 7, 887: 8, 888: 8, 892: 8, 923: 2, 929: 4, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1033: 5, 1034: 5, 1036: 8, 1039: 8, 1057: 5, 1064: 7, 1108: 8, 1365: 5, 1424: 5, 1729: 1
-  }],
-  CAR.CIVIC: [{
-    57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 450: 8, 464: 8, 470: 2, 476: 7, 487: 4, 490: 8, 493: 5, 506: 8, 512: 6, 513: 6, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1633: 8,
-  }],
-  CAR.CIVIC_BOSCH: [{
-    57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 460: 3, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 829: 5, 862: 8, 884: 8, 891: 8, 892: 8, 927: 8, 929: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1108: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1625: 5, 1629: 5, 1633: 8,
-  },
-  {
-    57: 3, 148: 8, 228: 5, 304: 8, 330: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 450: 8, 464: 8, 470: 2, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 506: 8, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 815: 8, 825: 4, 829: 5, 846: 8, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 892: 8, 918: 7, 927: 8, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1092: 1, 1108: 8, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1633: 8
-  }],
-  CAR.CRV_5G: [{
-    57: 3, 148: 8, 199: 4, 228: 5, 231: 5, 232: 7, 304: 8, 330: 8, 340: 8, 344: 8, 380: 8, 399: 7, 401: 8, 420: 8, 423: 2, 427: 3, 428: 8, 432: 7, 441: 5, 446: 3, 450: 8, 464: 8, 467: 2, 469: 3, 470: 2, 474: 8, 476: 7, 477: 8, 479: 8, 490: 8, 493: 5, 495: 8, 507: 1, 545: 6, 597: 8, 661: 4, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 814: 4, 815: 8, 817: 4, 825: 4, 829: 5, 862: 8, 881: 8, 882: 4, 884: 8, 888: 8, 891: 8, 927: 8, 918: 7, 929: 8, 983: 8, 985: 3, 1024: 5, 1027: 5, 1029: 8, 1036: 8, 1039: 8, 1064: 7, 1108: 8, 1092: 1, 1115: 2, 1125: 8, 1127: 2, 1296: 8, 1302: 8, 1322: 5, 1361: 5, 1365: 5, 1424: 5, 1600: 5, 1601: 8, 1618: 5, 1633: 8, 1670: 5
-  }],
-  CAR.ODYSSEY: [{
-    57: 3, 148: 8, 228: 5, 229: 4, 316: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1619: 5, 1623: 5, 1668: 5
-  },
-  {
-    57: 3, 148: 8, 228: 5, 229: 4, 304: 8, 342: 6, 344: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 427: 3, 432: 7, 440: 8, 450: 8, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 507: 1, 542: 7, 545: 6, 597: 8, 662: 4, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 806: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 862: 8, 871: 8, 881: 8, 882: 4, 884: 8, 891: 8, 892: 8, 905: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1029: 8, 1036: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1302: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1616: 5, 1619: 5, 1623: 5, 1668: 5
-  }],
   CAR.ODYSSEY_CHN: [{
     57: 3, 145: 8, 316: 8, 342: 6, 344: 8, 380: 8, 398: 3, 399: 7, 401: 8, 404: 4, 411: 5, 420: 8, 422: 8, 423: 2, 426: 8, 432: 7, 450: 8, 464: 8, 490: 8, 506: 8, 507: 1, 512: 6, 513: 6, 597: 8, 610: 8, 611: 8, 612: 8, 617: 8, 660: 8, 661: 4, 773: 7, 780: 8, 804: 8, 808: 8, 829: 5, 862: 8, 884: 7, 892: 8, 923: 2, 929: 8, 1030: 5, 1137: 8, 1302: 8, 1348: 5, 1361: 5, 1365: 5, 1600: 5, 1601: 8, 1639: 8
   }],
-  CAR.PILOT_2019: [{
-    57: 3, 145: 8, 228: 5, 308: 5, 316: 8, 334: 8, 342: 6, 344: 8, 379: 8, 380: 8, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 463: 8, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 538: 3, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 825: 4, 829: 5, 837: 5, 856: 7, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 983: 8, 985: 3, 1029: 8, 1052: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1110: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1614: 5, 1615: 8, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
-  },
-  {
-    57: 3, 145: 8, 228: 5, 229: 4, 308: 5, 316: 8, 339: 7, 342: 6, 344: 8, 380: 8, 392: 6, 399: 7, 411: 5, 419: 8, 420: 8, 422: 8, 425: 8, 426: 8, 427: 3, 432: 7, 464: 8, 476: 4, 490: 8, 506: 8, 512: 6, 513: 6, 542: 7, 545: 5, 546: 3, 597: 8, 660: 8, 773: 7, 777: 8, 780: 8, 795: 8, 800: 8, 804: 8, 808: 8, 817: 4, 819: 7, 821: 5, 829: 5, 871: 8, 881: 8, 882: 2, 884: 7, 891: 8, 892: 8, 923: 2, 927: 8, 929: 8, 963: 8, 965: 8, 966: 8, 967: 8, 983: 8, 985: 3, 1027: 5, 1029: 8, 1039: 8, 1064: 7, 1088: 8, 1089: 8, 1092: 1, 1108: 8, 1125: 8, 1296: 8, 1424: 5, 1445: 8, 1600: 5, 1601: 8, 1612: 5, 1613: 5, 1616: 5, 1617: 8, 1618: 5, 1623: 5, 1668: 5
-  }],
 }
 
 # add DIAG_MSGS to fingerprints
@@ -413,6 +386,7 @@ FW_VERSIONS = {
       b'37805-5AG-Z910\x00\x00',
       b'37805-5AJ-A750\x00\x00',
       b'37805-5AJ-L750\x00\x00',
+      b'37805-5AK-T530\x00\x00',
       b'37805-5AN-A750\x00\x00',
       b'37805-5AN-A830\x00\x00',
       b'37805-5AN-A840\x00\x00',
@@ -473,6 +447,7 @@ FW_VERSIONS = {
       b'28101-5DJ-A710\x00\x00',
       b'28101-5DV-E330\x00\x00',
       b'28101-5DV-E610\x00\x00',
+      b'28101-5DV-E820\x00\x00',
     ],
     (Ecu.vsa, 0x18da28f1, None): [
       b'57114-TBG-A330\x00\x00',
@@ -504,6 +479,7 @@ FW_VERSIONS = {
       b'77959-TGG-A020\x00\x00',
       b'77959-TGG-A030\x00\x00',
       b'77959-TGG-G010\x00\x00',
+      b'77959-TGG-G110\x00\x00',
       b'77959-TGG-J320\x00\x00',
       b'77959-TGG-Z820\x00\x00',
     ],
@@ -534,6 +510,7 @@ FW_VERSIONS = {
       b'78109-TGL-G120\x00\x00',
       b'78109-TGL-G130\x00\x00',
       b'78109-TGL-G230\x00\x00',
+      b'78109-TGL-GM10\x00\x00',
     ],
     (Ecu.fwdRadar, 0x18dab0f1, None): [
       b'36802-TBA-A150\x00\x00',
@@ -556,6 +533,7 @@ FW_VERSIONS = {
       b'36161-TGG-A120\x00\x00',
       b'36161-TGG-G050\x00\x00',
       b'36161-TGG-G130\x00\x00',
+      b'36161-TGG-G140\x00\x00',
       b'36161-TGK-Q120\x00\x00',
       b'36161-TGL-G050\x00\x00',
       b'36161-TGL-G070\x00\x00',
@@ -838,6 +816,29 @@ FW_VERSIONS = {
       b'77959-T5R-A230\x00\x00',
     ],
   },
+  CAR.FREED: {
+    (Ecu.gateway, 0x18daeff1, None): [
+      b'38897-TDK-J010\x00\x00',
+    ],
+    (Ecu.eps, 0x18da30f1, None): [
+      b'39990-TDK-J050\x00\x00',
+      b'39990-TDK-N020\x00\x00',
+    ],
+    # TODO: vsa is "essential" for fpv2 but doesn't appear on some models
+    (Ecu.vsa, 0x18da28f1, None): [
+      b'57114-TDK-J120\x00\x00',
+      b'57114-TDK-J330\x00\x00',
+    ],
+    (Ecu.combinationMeter, 0x18da60f1, None): [
+      b'78109-TDK-J310\x00\x00',
+      b'78109-TDK-J320\x00\x00',
+    ],
+    (Ecu.fwdRadar, 0x18dab0f1, None): [
+      b'36161-TDK-J070\x00\x00',
+      b'36161-TDK-J080\x00\x00',
+      b'36161-TDK-J530\x00\x00',
+    ],
+  },
   CAR.ODYSSEY: {
     (Ecu.gateway, 0x18daeff1, None): [
       b'38897-THR-A010\x00\x00',
@@ -908,6 +909,7 @@ FW_VERSIONS = {
       b'78109-THR-AB30\x00\x00',
       b'78109-THR-AB40\x00\x00',
       b'78109-THR-AC20\x00\x00',
+      b'78109-THR-AC30\x00\x00',
       b'78109-THR-AC40\x00\x00',
       b'78109-THR-AC50\x00\x00',
       b'78109-THR-AE20\x00\x00',
@@ -1033,6 +1035,7 @@ FW_VERSIONS = {
       b'78109-TGS-AK20\x00\x00',
       b'78109-TGS-AP20\x00\x00',
       b'78109-TGT-AJ20\x00\x00',
+      b'78109-TG7-AT20\x00\x00',
     ],
     (Ecu.vsa, 0x18da28f1, None): [
       b'57114-TG7-A630\x00\x00',
@@ -1144,6 +1147,7 @@ FW_VERSIONS = {
       b'78109-T6Z-A420\x00\x00',
       b'78109-T6Z-A510\x00\x00',
       b'78109-T6Z-A710\x00\x00',
+      b'78109-T6Z-A810\x00\x00',
       b'78109-T6Z-A910\x00\x00',
       b'78109-T6Z-AA10\x00\x00',
       b'78109-T6Z-C620\x00\x00',
@@ -1274,6 +1278,7 @@ DBC = {
   CAR.CRV_EU: dbc_dict('honda_crv_executive_2016_can_generated', 'acura_ilx_2016_nidec'),
   CAR.CRV_HYBRID: dbc_dict('honda_crv_hybrid_2019_can_generated', None),
   CAR.FIT: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
+  CAR.FREED: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
   CAR.HRV: dbc_dict('honda_fit_ex_2018_can_generated', 'acura_ilx_2016_nidec'),
   CAR.ODYSSEY: dbc_dict('honda_odyssey_exl_2018_generated', 'acura_ilx_2016_nidec'),
   CAR.ODYSSEY_CHN: dbc_dict('honda_odyssey_extreme_edition_2018_china_can_generated', 'acura_ilx_2016_nidec'),
@@ -1302,4 +1307,4 @@ SPEED_FACTOR = {
 
 HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
 HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
-HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
\ No newline at end of file
+HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py
index 026b2dada..19ff38129 100644
--- a/selfdrive/car/hyundai/carcontroller.py
+++ b/selfdrive/car/hyundai/carcontroller.py
@@ -1,11 +1,14 @@
 from cereal import car
 from common.realtime import DT_CTRL
+from common.numpy_fast import clip, interp
+from selfdrive.config import Conversions as CV
 from selfdrive.car import apply_std_steer_torque_limits
-from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc
+from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11, create_lfahda_mfc, create_acc_commands, create_acc_opt, create_frt_radar_opt
 from selfdrive.car.hyundai.values import Buttons, CarControllerParams, CAR
 from opendbc.can.packer import CANPacker
 
 VisualAlert = car.CarControl.HUDControl.VisualAlert
+LongCtrlState = car.CarControl.Actuators.LongControlState
 
 
 def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
@@ -42,7 +45,7 @@ class CarController():
     self.steer_rate_limited = False
     self.last_resume_frame = 0
 
-  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert,
+  def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
              left_lane, right_lane, left_lane_depart, right_lane_depart):
     # Steering Torque
     new_steer = int(round(actuators.steer * self.p.STEER_MAX))
@@ -62,24 +65,54 @@ class CarController():
                         left_lane, right_lane, left_lane_depart, right_lane_depart)
 
     can_sends = []
+
+    # tester present - w/ no response (keeps radar disabled)
+    if CS.CP.openpilotLongitudinalControl:
+      if (frame % 100) == 0:
+        can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
+
     can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, lkas_active,
                                    CS.lkas11, sys_warning, sys_state, enabled,
                                    left_lane, right_lane,
                                    left_lane_warning, right_lane_warning))
 
-    if pcm_cancel_cmd:
-      can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
-    elif CS.out.cruiseState.standstill:
-      # send resume at a max freq of 10Hz
-      if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
-        # send 25 messages at a time to increases the likelihood of resume being accepted
-        can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
-        self.last_resume_frame = frame
+    if not CS.CP.openpilotLongitudinalControl:
+      if pcm_cancel_cmd:
+        can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
+      elif CS.out.cruiseState.standstill:
+        # send resume at a max freq of 10Hz
+        if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
+          # send 25 messages at a time to increases the likelihood of resume being accepted
+          can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
+          self.last_resume_frame = frame
+
+    if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
+      lead_visible = False
+      accel = actuators.accel if enabled else 0
+
+      jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
+
+      if accel < 0:
+        accel = interp(accel - CS.out.aEgo, [-1.0, -0.5], [2 * accel, accel])
+
+      accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+
+      stopping = (actuators.longControlState == LongCtrlState.stopping)
+      set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
+      can_sends.extend(create_acc_commands(self.packer, enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
 
     # 20 Hz LFA MFA message
     if frame % 5 == 0 and self.car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
                                                    CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
-                                                   CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
+                                                   CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]:
       can_sends.append(create_lfahda_mfc(self.packer, enabled))
 
+    # 5 Hz ACC options
+    if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
+      can_sends.extend(create_acc_opt(self.packer))
+
+    # 2 Hz front radar options
+    if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
+      can_sends.append(create_frt_radar_opt(self.packer))
+
     return can_sends
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 2cb176aa1..7cc62b3cb 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -49,6 +49,7 @@ class CarState(CarStateBase):
 
     # cruise state
     if self.CP.openpilotLongitudinalControl:
+      # These are not used for engage/disengage since openpilot keeps track of state using the buttons
       ret.cruiseState.available = cp.vl["TCS13"]["ACCEnable"] == 0
       ret.cruiseState.enabled = cp.vl["TCS13"]["ACC_REQ"] == 1
       ret.cruiseState.standstill = False
@@ -57,11 +58,11 @@ class CarState(CarStateBase):
       ret.cruiseState.enabled = cp.vl["SCC12"]["ACCMode"] != 0
       ret.cruiseState.standstill = cp.vl["SCC11"]["SCCInfoDisplay"] == 4.
 
-    if ret.cruiseState.enabled:
-      speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
-      ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
-    else:
-      ret.cruiseState.speed = 0
+      if ret.cruiseState.enabled:
+        speed_conv = CV.MPH_TO_MS if cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"] else CV.KPH_TO_MS
+        ret.cruiseState.speed = cp.vl["SCC11"]["VSetDis"] * speed_conv
+      else:
+        ret.cruiseState.speed = 0
 
     # TODO: Find brake pressure
     ret.brake = 0
@@ -90,12 +91,13 @@ class CarState(CarStateBase):
 
     ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear))
 
-    if self.CP.carFingerprint in FEATURES["use_fca"]:
-      ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
-      ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
-    else:
-      ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
-      ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
+    if not self.CP.openpilotLongitudinalControl:
+      if self.CP.carFingerprint in FEATURES["use_fca"]:
+        ret.stockAeb = cp.vl["FCA11"]["FCA_CmdAct"] != 0
+        ret.stockFcw = cp.vl["FCA11"]["CF_VSM_Warn"] == 2
+      else:
+        ret.stockAeb = cp.vl["SCC12"]["AEB_CmdAct"] != 0
+        ret.stockFcw = cp.vl["SCC12"]["CF_VSM_Warn"] == 2
 
     if self.CP.enableBsm:
       ret.leftBlindspot = cp.vl["LCA11"]["CF_Lca_IndLeft"] != 0
@@ -106,7 +108,6 @@ class CarState(CarStateBase):
     self.clu11 = copy.copy(cp.vl["CLU11"])
     self.park_brake = cp.vl["TCS13"]["PBRAKE_ACT"] == 1
     self.steer_state = cp.vl["MDPS12"]["CF_Mdps_ToiActive"]  # 0 NOT ACTIVE, 1 ACTIVE
-    self.lead_distance = cp.vl["SCC11"]["ACC_ObjDist"]
     self.brake_hold = cp.vl["TCS15"]["AVH_LAMP"] == 2 # 0 OFF, 1 ERROR, 2 ACTIVE, 3 READY
     self.brake_error = cp.vl["TCS13"]["ACCEnable"] != 0 # 0 ACC CONTROL ENABLED, 1-3 ACC CONTROL DISABLED
     self.prev_cruise_buttons = self.cruise_buttons
@@ -168,12 +169,6 @@ class CarState(CarStateBase):
 
       ("SAS_Angle", "SAS11", 0),
       ("SAS_Speed", "SAS11", 0),
-
-      ("MainMode_ACC", "SCC11", 0),
-      ("VSetDis", "SCC11", 0),
-      ("SCCInfoDisplay", "SCC11", 0),
-      ("ACC_ObjDist", "SCC11", 0),
-      ("ACCMode", "SCC12", 1),
     ]
 
     checks = [
@@ -191,11 +186,31 @@ class CarState(CarStateBase):
     ]
 
     if not CP.openpilotLongitudinalControl:
+      signals += [
+        ("MainMode_ACC", "SCC11", 0),
+        ("VSetDis", "SCC11", 0),
+        ("SCCInfoDisplay", "SCC11", 0),
+        ("ACC_ObjDist", "SCC11", 0),
+        ("ACCMode", "SCC12", 1),
+      ]
+
       checks += [
         ("SCC11", 50),
         ("SCC12", 50),
       ]
 
+      if CP.carFingerprint in FEATURES["use_fca"]:
+        signals += [
+          ("FCA_CmdAct", "FCA11", 0),
+          ("CF_VSM_Warn", "FCA11", 0),
+        ]
+        checks += [("FCA11", 50)]
+      else:
+        signals += [
+          ("AEB_CmdAct", "SCC12", 0),
+          ("CF_VSM_Warn", "SCC12", 0),
+        ]
+
     if CP.enableBsm:
       signals += [
         ("CF_Lca_IndLeft", "LCA11", 0),
@@ -250,19 +265,6 @@ class CarState(CarStateBase):
         ("LVR12", 100)
       ]
 
-    if CP.carFingerprint in FEATURES["use_fca"]:
-      signals += [
-        ("FCA_CmdAct", "FCA11", 0),
-        ("CF_VSM_Warn", "FCA11", 0),
-      ]
-      if not CP.openpilotLongitudinalControl:
-        checks += [("FCA11", 50)]
-    else:
-      signals += [
-        ("AEB_CmdAct", "SCC12", 0),
-        ("CF_VSM_Warn", "SCC12", 0),
-      ]
-
     return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0)
 
   @staticmethod
diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py
index 77b47e8ed..dd43ec1a8 100644
--- a/selfdrive/car/hyundai/hyundaican.py
+++ b/selfdrive/car/hyundai/hyundaican.py
@@ -3,7 +3,6 @@ from selfdrive.car.hyundai.values import CAR, CHECKSUM
 
 hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf)
 
-
 def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
                   lkas11, sys_warning, sys_state, enabled,
                   left_lane, right_lane,
@@ -19,7 +18,7 @@ def create_lkas11(packer, frame, car_fingerprint, apply_steer, steer_req,
 
   if car_fingerprint in [CAR.SONATA, CAR.PALISADE, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021, CAR.SANTA_FE,
                          CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
-                         CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV]:
+                         CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_EV, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]:
     values["CF_Lkas_LdwsActivemode"] = int(left_lane) + (int(right_lane) << 1)
     values["CF_Lkas_LdwsOpt_USM"] = 2
 
@@ -78,7 +77,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
   }
   return packer.make_can_msg("LFAHDA_MFC", 0, values)
 
-def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, stopping):
+def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping):
   commands = []
 
   scc11_values = {
@@ -86,14 +85,19 @@ def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, st
     "TauGapSet": 4,
     "VSetDis": set_speed if enabled else 0,
     "AliveCounterACC": idx % 0x10,
+    "ObjValid": 1 if lead_visible else 0,
+    "ACC_ObjStatus": 1 if lead_visible else 0,
+    "ACC_ObjLatPos": 0,
+    "ACC_ObjRelSpd": 0,
+    "ACC_ObjDist": 0,
   }
   commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
 
   scc12_values = {
     "ACCMode": 1 if enabled else 0,
-    "StopReq": 1 if stopping else 0,
-    "aReqRaw": accel,
-    "aReqValue": accel, # stock ramps up at 1.0/s and down at 0.5/s until it reaches aReqRaw
+    "StopReq": 1 if enabled and stopping else 0,
+    "aReqRaw": accel if enabled else 0,
+    "aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
     "CR_VSM_Alive": idx % 0xF,
   }
   scc12_dat = packer.make_can_msg("SCC12", 0, scc12_values)[2]
@@ -104,10 +108,10 @@ def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, st
   scc14_values = {
     "ComfortBandUpper": 0.0, # stock usually is 0 but sometimes uses higher values
     "ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
-    "JerkUpperLimit": 1.0 if enabled else 0, # stock usually is 1.0 but sometimes uses higher values
-    "JerkLowerLimit": 0.5 if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
+    "JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values
+    "JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
     "ACCMode": 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
-    "ObjGap": 3 if lead_visible else 0, # TODO: 1-5 based on distance to lead vehicle
+    "ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
   }
   commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
 
@@ -117,6 +121,9 @@ def create_acc_commands(packer, enabled, accel, idx, lead_visible, set_speed, st
     # test: [(idx % 0xF, -((idx % 0xF) + 2) % 4) for idx in range(0x14)]
     "CR_FCA_Alive": ((-((idx % 0xF) + 2) % 4) << 2) + 1,
     "Supplemental_Counter": idx % 0xF,
+    "PAINT1_Status": 1,
+    "FCA_DrvSetStatus": 1,
+    "FCA_Status": 1, # AEB disabled
   }
   fca11_dat = packer.make_can_msg("FCA11", 0, fca11_values)[2]
   fca11_values["CR_FCA_ChkSum"] = 0x10 - sum([sum(divmod(i, 16)) for i in fca11_dat]) % 0x10
@@ -135,10 +142,8 @@ def create_acc_opt(packer):
   commands.append(packer.make_can_msg("SCC13", 0, scc13_values))
 
   fca12_values = {
-    # stock values may be needed if openpilot has vision based AEB some day
-    # for now we are not setting these because there is no AEB for vision only
-    # "FCA_USM": 3,
-    # "FCA_DrvSetState": 2,
+    "FCA_DrvSetState": 2,
+    "FCA_USM": 1, # AEB disabled
   }
   commands.append(packer.make_can_msg("FCA12", 0, fca12_values))
 
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index 1fee6e58b..b738beb9f 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -1,30 +1,51 @@
 #!/usr/bin/env python3
 from cereal import car
+from panda import Panda
+from common.params import Params
 from selfdrive.config import Conversions as CV
-from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car.hyundai.values import CAR, EV_CAR, HYBRID_CAR, Buttons, CarControllerParams
+from selfdrive.car.hyundai.radar_interface import RADAR_START_ADDR
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
+from selfdrive.car.disable_ecu import disable_ecu
+
+ButtonType = car.CarState.ButtonEvent.Type
+EventName = car.CarEvent.EventName
 
 class CarInterface(CarInterfaceBase):
+  @staticmethod
+  def get_pid_accel_limits(CP, current_speed, cruise_speed):
+    return CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX
+
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=[]):  # pylint: disable=dangerous-default-value
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "hyundai"
-    ret.safetyModel = car.CarParams.SafetyModel.hyundai
-    ret.radarOffCan = True
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundai, 0)]
+    ret.radarOffCan = RADAR_START_ADDR not in fingerprint[1]
 
-    # Most Hyundai car ports are community features for now
-    ret.communityFeature = candidate not in [CAR.SONATA, CAR.PALISADE]
+    # WARNING: disabling radar also disables AEB (and we show the same warning on the instrument cluster as if you manually disabled AEB)
+    ret.openpilotLongitudinalControl = Params().get_bool("DisableRadar") and candidate in [CAR.SONATA, CAR.SONATA_HYBRID, CAR.PALISADE, CAR.SANTA_FE]
+
+    ret.pcmCruise = not ret.openpilotLongitudinalControl
 
     ret.steerActuatorDelay = 0.1  # Default delay
     ret.steerRateCost = 0.5
     ret.steerLimitTimer = 0.4
     tire_stiffness_factor = 1.
 
-    ret.startAccel = 1.0
+    ret.stoppingControl = True
+    ret.vEgoStopping = 1.0
 
-    if candidate == CAR.SANTA_FE:
+    ret.longitudinalTuning.kpV = [0.1]
+    ret.longitudinalTuning.kiV = [0.0]
+    ret.stopAccel = 0.0
+    ret.startAccel = 0.0
+
+    ret.longitudinalActuatorDelayUpperBound = 1.0 # s
+
+    if candidate in [CAR.SANTA_FE, CAR.SANTA_FE_2022]:
       ret.lateralTuning.pid.kf = 0.00005
       ret.mass = 3982. * CV.LB_TO_KG + STD_CARGO_KG
       ret.wheelbase = 2.766
@@ -53,6 +74,7 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1999. + STD_CARGO_KG
       ret.wheelbase = 2.90
       ret.steerRatio = 15.6 * 1.15
+      tire_stiffness_factor = 0.63
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
     elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
@@ -103,7 +125,7 @@ class CarInterface(CarInterfaceBase):
       tire_stiffness_factor = 0.385
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-    elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
+    elif candidate in [CAR.IONIQ, CAR.IONIQ_EV_LTD, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022]:
       ret.lateralTuning.pid.kf = 0.00006
       ret.mass = 1490. + STD_CARGO_KG  # weight per hyundai site https://www.hyundaiusa.com/ioniq-electric/specifications.aspx
       ret.wheelbase = 2.7
@@ -111,7 +133,7 @@ class CarInterface(CarInterfaceBase):
       tire_stiffness_factor = 0.385
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
-      if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV]:
+      if candidate not in [CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.IONIQ_HEV_2022]:
         ret.minSteerSpeed = 32 * CV.MPH_TO_MS
     elif candidate == CAR.VELOSTER:
       ret.lateralTuning.pid.kf = 0.00005
@@ -186,6 +208,14 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kf = 0.00005
       ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
+    elif candidate == CAR.KIA_K5_2021:
+      ret.lateralTuning.pid.kf = 0.00005
+      ret.mass = 3228. * CV.LB_TO_KG
+      ret.wheelbase = 2.85
+      ret.steerRatio = 13.27  # 2021 Kia K5 Steering Ratio (all trims)
+      tire_stiffness_factor = 0.5
+      ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
+      ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.05]]
 
     # Genesis
     elif candidate == CAR.GENESIS_G70:
@@ -218,15 +248,15 @@ class CarInterface(CarInterfaceBase):
 
     # these cars require a special panda safety mode due to missing counters and checksums in the messages
     if candidate in [CAR.HYUNDAI_GENESIS, CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ_PHEV, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_SORENTO,
-                     CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER, CAR.KIA_SELTOS,
-                     CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA]:
-      ret.safetyModel = car.CarParams.SafetyModel.hyundaiLegacy
+                     CAR.SONATA_LF, CAR.KIA_NIRO_EV, CAR.KIA_OPTIMA, CAR.VELOSTER, CAR.KIA_STINGER,
+                     CAR.GENESIS_G70, CAR.GENESIS_G80, CAR.KIA_CEED, CAR.ELANTRA, CAR.IONIQ_HEV_2022]:
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.hyundaiLegacy)]
 
     # set appropriate safety param for gas signal
     if candidate in HYBRID_CAR:
-      ret.safetyParam = 2
+      ret.safetyConfigs[0].safetyParam = 2
     elif candidate in EV_CAR:
-      ret.safetyParam = 1
+      ret.safetyConfigs[0].safetyParam = 1
 
     ret.centerToFront = ret.wheelbase * 0.4
 
@@ -241,8 +271,16 @@ class CarInterface(CarInterfaceBase):
 
     ret.enableBsm = 0x58b in fingerprint[0]
 
+    if ret.openpilotLongitudinalControl:
+      ret.safetyConfigs[0].safetyParam |= Panda.FLAG_HYUNDAI_LONG
+
     return ret
 
+  @staticmethod
+  def init(CP, logcan, sendcan):
+    if CP.openpilotLongitudinalControl:
+      disable_ecu(logcan, sendcan, addr=0x7d0, com_cont_req=b'\x28\x83\x01')
+
   def update(self, c, can_strings):
     self.cp.update_strings(can_strings)
     self.cp_cam.update_strings(can_strings)
@@ -251,7 +289,46 @@ class CarInterface(CarInterfaceBase):
     ret.canValid = self.cp.can_valid and self.cp_cam.can_valid
     ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False
 
-    events = self.create_common_events(ret)
+    events = self.create_common_events(ret, pcm_enable=self.CS.CP.pcmCruise)
+
+    if self.CS.brake_error:
+      events.add(EventName.brakeUnavailable)
+    if self.CS.brake_hold and self.CS.CP.openpilotLongitudinalControl:
+      events.add(EventName.brakeHold)
+    if self.CS.park_brake:
+      events.add(EventName.parkBrake)
+
+    if self.CS.CP.openpilotLongitudinalControl:
+      buttonEvents = []
+
+      if self.CS.cruise_buttons != self.CS.prev_cruise_buttons:
+        be = car.CarState.ButtonEvent.new_message()
+        be.type = ButtonType.unknown
+        if self.CS.cruise_buttons != 0:
+          be.pressed = True
+          but = self.CS.cruise_buttons
+        else:
+          be.pressed = False
+          but = self.CS.prev_cruise_buttons
+        if but == Buttons.RES_ACCEL:
+          be.type = ButtonType.accelCruise
+        elif but == Buttons.SET_DECEL:
+          be.type = ButtonType.decelCruise
+        elif but == Buttons.GAP_DIST:
+          be.type = ButtonType.gapAdjustCruise
+        elif but == Buttons.CANCEL:
+          be.type = ButtonType.cancel
+        buttonEvents.append(be)
+
+        ret.buttonEvents = buttonEvents
+
+        for b in ret.buttonEvents:
+          # do enable on both accel and decel buttons
+          if b.type in [ButtonType.accelCruise, ButtonType.decelCruise] and not b.pressed:
+            events.add(EventName.buttonEnable)
+          # do disable on button down
+          if b.type == ButtonType.cancel and b.pressed:
+            events.add(EventName.buttonCancel)
 
     # low speed steer alert hysteresis logic (only for cars with steer cut off above 10 m/s)
     if ret.vEgo < (self.CP.minSteerSpeed + 2.) and self.CP.minSteerSpeed > 10.:
@@ -268,7 +345,7 @@ class CarInterface(CarInterfaceBase):
 
   def apply(self, c):
     can_sends = self.CC.update(c.enabled, self.CS, self.frame, c.actuators,
-                               c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.leftLaneVisible,
+                               c.cruiseControl.cancel, c.hudControl.visualAlert, c.hudControl.setSpeed, c.hudControl.leftLaneVisible,
                                c.hudControl.rightLaneVisible, c.hudControl.leftLaneDepart, c.hudControl.rightLaneDepart)
     self.frame += 1
     return can_sends
diff --git a/selfdrive/car/hyundai/radar_interface.py b/selfdrive/car/hyundai/radar_interface.py
index fdbe837af..6022069a2 100644
--- a/selfdrive/car/hyundai/radar_interface.py
+++ b/selfdrive/car/hyundai/radar_interface.py
@@ -1,36 +1,47 @@
 #!/usr/bin/env python3
+import math
+
 from cereal import car
 from opendbc.can.parser import CANParser
 from selfdrive.car.interfaces import RadarInterfaceBase
 from selfdrive.car.hyundai.values import DBC
 
+RADAR_START_ADDR = 0x500
+RADAR_MSG_COUNT = 32
+
 
 def get_radar_can_parser(CP):
-  signals = [
-    # sig_name, sig_address, default
-    ("ACC_ObjStatus", "SCC11", 0),
-    ("ACC_ObjLatPos", "SCC11", 0),
-    ("ACC_ObjDist", "SCC11", 0),
-    ("ACC_ObjRelSpd", "SCC11", 0),
-  ]
-  checks = [
-    # address, frequency
-    ("SCC11", 50),
-  ]
-  return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+  if DBC[CP.carFingerprint]['radar'] is None:
+    return None
+
+  signals = []
+  checks = []
+
+  for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
+    msg = f"RADAR_TRACK_{addr:x}"
+    signals += [
+      ("STATE", msg, 0),
+      ("AZIMUTH", msg, 0),
+      ("LONG_DIST", msg, 0),
+      ("REL_ACCEL", msg, 0),
+      ("REL_SPEED", msg, 0),
+    ]
+    checks += [(msg, 50)]
+  return CANParser(DBC[CP.carFingerprint]['radar'], signals, checks, 1)
 
 
 class RadarInterface(RadarInterfaceBase):
   def __init__(self, CP):
     super().__init__(CP)
-    self.rcp = get_radar_can_parser(CP)
     self.updated_messages = set()
-    self.trigger_msg = 0x420
+    self.trigger_msg = RADAR_START_ADDR + RADAR_MSG_COUNT - 1
     self.track_id = 0
+
     self.radar_off_can = CP.radarOffCan
+    self.rcp = get_radar_can_parser(CP)
 
   def update(self, can_strings):
-    if self.radar_off_can:
+    if self.radar_off_can or (self.rcp is None):
       return super().update(None)
 
     vls = self.rcp.update_strings(can_strings)
@@ -46,25 +57,35 @@ class RadarInterface(RadarInterfaceBase):
 
   def _update(self, updated_messages):
     ret = car.RadarData.new_message()
-    cpt = self.rcp.vl
+    if self.rcp is None:
+      return ret
+
     errors = []
+
     if not self.rcp.can_valid:
       errors.append("canError")
     ret.errors = errors
 
-    valid = cpt["SCC11"]['ACC_ObjStatus']
-    if valid:
-      for ii in range(2):
-        if ii not in self.pts:
-          self.pts[ii] = car.RadarData.RadarPoint.new_message()
-          self.pts[ii].trackId = self.track_id
-          self.track_id += 1
-        self.pts[ii].dRel = cpt["SCC11"]['ACC_ObjDist']  # from front of car
-        self.pts[ii].yRel = -cpt["SCC11"]['ACC_ObjLatPos']  # in car frame's y axis, left is negative
-        self.pts[ii].vRel = cpt["SCC11"]['ACC_ObjRelSpd']
-        self.pts[ii].aRel = float('nan')
-        self.pts[ii].yvRel = float('nan')
-        self.pts[ii].measured = True
+    for addr in range(RADAR_START_ADDR, RADAR_START_ADDR + RADAR_MSG_COUNT):
+      msg = self.rcp.vl[f"RADAR_TRACK_{addr:x}"]
+
+      if addr not in self.pts:
+        self.pts[addr] = car.RadarData.RadarPoint.new_message()
+        self.pts[addr].trackId = self.track_id
+        self.track_id += 1
+
+      valid = msg['STATE'] in [3, 4]
+      if valid:
+        azimuth = math.radians(msg['AZIMUTH'])
+        self.pts[addr].measured = True
+        self.pts[addr].dRel = math.cos(azimuth) * msg['LONG_DIST']
+        self.pts[addr].yRel = 0.5 * -math.sin(azimuth) * msg['LONG_DIST']
+        self.pts[addr].vRel = msg['REL_SPEED']
+        self.pts[addr].aRel = msg['REL_ACCEL'] 
+        self.pts[addr].yvRel = float('nan')
+
+      else:
+        del self.pts[addr]
 
     ret.points = list(self.pts.values())
     return ret
diff --git a/selfdrive/car/hyundai/values.py b/selfdrive/car/hyundai/values.py
index 900000108..2f2995e6b 100644
--- a/selfdrive/car/hyundai/values.py
+++ b/selfdrive/car/hyundai/values.py
@@ -6,10 +6,13 @@ Ecu = car.CarParams.Ecu
 
 # Steer torque limits
 class CarControllerParams:
+  ACCEL_MIN = -3.5 # m/s
+  ACCEL_MAX = 2.0 # m/s
+
   def __init__(self, CP):
     if CP.carFingerprint in [CAR.SONATA, CAR.PALISADE, CAR.SANTA_FE, CAR.VELOSTER, CAR.GENESIS_G70,
                              CAR.IONIQ_EV_2020, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.ELANTRA_2021,
-                             CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV]:
+                             CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.KONA_EV, CAR.KONA, CAR.IONIQ_HEV_2022]:
       self.STEER_MAX = 384
     else:
       self.STEER_MAX = 255
@@ -19,7 +22,6 @@ class CarControllerParams:
     self.STEER_DRIVER_MULTIPLIER = 2
     self.STEER_DRIVER_FACTOR = 1
 
-
 class CAR:
   # Hyundai
   ELANTRA = "HYUNDAI ELANTRA 2017"
@@ -28,6 +30,7 @@ class CAR:
   ELANTRA_GT_I30 = "HYUNDAI I30 N LINE 2019 & GT 2018 DCT"
   HYUNDAI_GENESIS = "HYUNDAI GENESIS 2015-2016"
   IONIQ = "HYUNDAI IONIQ HYBRID 2017-2019"
+  IONIQ_HEV_2022 = "HYUNDAI IONIQ HYBRID 2020-2022"
   IONIQ_EV_LTD = "HYUNDAI IONIQ ELECTRIC LIMITED 2019"
   IONIQ_EV_2020 = "HYUNDAI IONIQ ELECTRIC 2020"
   IONIQ_PHEV = "HYUNDAI IONIQ PHEV 2020"
@@ -35,6 +38,7 @@ class CAR:
   KONA_EV = "HYUNDAI KONA ELECTRIC 2019"
   KONA_HEV = "HYUNDAI KONA HYBRID 2020"
   SANTA_FE = "HYUNDAI SANTA FE 2019"
+  SANTA_FE_2022 = "HYUNDAI SANTA FE 2022"
   SONATA = "HYUNDAI SONATA 2020"
   SONATA_LF = "HYUNDAI SONATA 2019"
   PALISADE = "HYUNDAI PALISADE 2020"
@@ -43,6 +47,7 @@ class CAR:
 
   # Kia
   KIA_FORTE = "KIA FORTE E 2018 & GT 2021"
+  KIA_K5_2021 = "KIA K5 2021"
   KIA_NIRO_EV = "KIA NIRO EV 2020"
   KIA_NIRO_HEV = "KIA NIRO HYBRID 2019"
   KIA_NIRO_HEV_2021 = "KIA NIRO HYBRID 2021"
@@ -133,9 +138,6 @@ FINGERPRINTS = {
   CAR.IONIQ_EV_2020: [{
     127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8
   }],
-  CAR.IONIQ_EV_LTD: [{
-    127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 7, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1168: 7, 1173: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1425: 2, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1507: 8, 1535: 8
-  }],
   CAR.IONIQ: [{
     68:8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 524: 8, 544: 8, 576:8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1155: 8, 1156: 8, 1157: 4, 1164: 8, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1379: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1473: 8, 1476: 8, 1507: 8, 1535: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2005: 8, 2008: 8, 2012: 8, 2013: 8
   }],
@@ -145,9 +147,6 @@ FINGERPRINTS = {
   CAR.KIA_NIRO_EV: [{
     127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 516: 8, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 8, 1151: 6, 1156: 8, 1157: 4, 1168: 7, 1173: 8, 1183: 8, 1186: 2, 1191: 2, 1193: 8, 1225: 8, 1260: 8, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1407: 8, 1419: 8, 1426: 8, 1427: 6, 1429: 8, 1430: 8, 1456: 4, 1470: 8, 1473: 8, 1507: 8, 1535: 8, 1990: 8, 1998: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
   }],
-  CAR.KIA_FORTE: [{
-    67: 8, 127: 8, 304: 8, 320: 8, 339: 8, 354: 3, 356: 4, 544: 8, 576: 8, 593: 8, 608: 8, 688: 5, 809: 8, 832: 8, 854: 7, 870: 7, 871: 8, 872: 8, 897: 8, 902: 8, 903: 8, 905: 8, 909: 8, 916: 8, 1040: 8, 1042: 8, 1056: 8, 1057: 8, 1078: 4, 1107: 5, 1136: 8, 1156: 8, 1170: 8, 1173: 8, 1186: 2, 1191: 2, 1225: 8, 1265: 4, 1280: 4, 1287: 4, 1290: 8, 1292: 8, 1294: 8, 1312: 8, 1322: 8, 1342: 6, 1345: 8, 1348: 8, 1363: 8, 1369: 8, 1384: 8, 1394: 8, 1407: 8, 1414: 3, 1419: 8, 1427: 6, 1456: 4, 1470: 8, 1988: 8, 1996: 8, 2000: 8, 2004: 8, 2008: 8, 2012: 8, 2015: 8
-  }],
   CAR.KIA_OPTIMA_H: [{
     68: 8, 127: 8, 304: 8, 320: 8, 339: 8, 352: 8, 356: 4, 544: 8, 593: 8, 688: 5, 832: 8, 881: 8, 882: 8, 897: 8, 902: 8, 903: 6, 916: 8, 1040: 8, 1056: 8, 1057: 8, 1078: 4, 1136: 6, 1151: 6, 1168: 7, 1173: 8, 1236: 2, 1265: 4, 1280: 1, 1287: 4, 1290: 8, 1291: 8, 1292: 8, 1322: 8, 1331: 8, 1332: 8, 1333: 8, 1342: 6, 1345: 8, 1348: 8, 1355: 8, 1363: 8, 1369: 8, 1371: 8, 1407: 8, 1419: 8, 1427: 6, 1429: 8, 1430: 8, 1448: 8, 1456: 4, 1470: 8, 1476: 8, 1535: 8
   },
@@ -161,21 +160,42 @@ FINGERPRINTS = {
 
 
 FW_VERSIONS = {
+  CAR.IONIQ: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00AEhe SCC H-CUP      1.01 1.01 96400-G2000         ',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00AE  MDPS C 1.00 1.07 56310/G2301 4AEHC107',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00AEH MFC  AT EUR LHD 1.00 1.00 95740-G2400 180222',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x816H6F2051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x816U3H1051\x00\x00\xf1\x006U3H0_C2\x00\x006U3H1051\x00\x00HAE0G16US2\x00\x00\x00\x00',
+    ],
+  },
   CAR.IONIQ_PHEV: {
     (Ecu.fwdRadar, 0x7d0, None): [
       b'\xf1\000AEhe SCC FHCUP      1.00 1.02 99110-G2100         ',
+      b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2200         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\000AE  MDPS C 1.00 1.01 56310/G2510 4APHC101',
+      b'\xf1\x00AE  MDPS C 1.00 1.01 56310/G2560 4APHC101',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\000AEP MFC  AT USA LHD 1.00 1.01 95740-G2600 190819',
+      b'\xf1\x00AEP MFC  AT EUR RHD 1.00 1.01 95740-G2600 190819',
     ],
     (Ecu.engine, 0x7e0, None): [
-      b'\xf1\x816H6F6051\000\000\000\000\000\000\000\000',
+      b'\xf1\x816H6F6051\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x816U3J9051\000\000\xf1\0006U3H1_C2\000\0006U3J9051\000\000PAE0G16NL0\x82zT\xd2',
+      b'\xf1\x816U3J8051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J8051\x00\x00PAETG16UL0\x00\x00\x00\x00',
     ],
   },
   CAR.IONIQ_EV_2020: {
@@ -201,10 +221,29 @@ FW_VERSIONS = {
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00AE  MDPS C 1.00 1.02 56310G7300\x00 4AEEC102',
       b'\xf1\x00AE  MDPS C 1.00 1.04 56310/G7501 4AEEC104',
+      b'\xf1\x00AE  MDPS C 1.00 1.03 56310/G7300 4AEEC103',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00AEE MFC  AT EUR LHD 1.00 1.00 95740-G7200 160418',
       b'\xf1\x00AEE MFC  AT USA LHD 1.00 1.00 95740-G2400 180222',
+      b'\xf1\x00AEE MFC  AT EUR LHD 1.00 1.00 95740-G2300 170703',
+    ],
+  },
+  CAR.IONIQ_HEV_2022: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00AEhe SCC F-CUP      1.00 1.00 99110-G2600         ',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00AE  MDPS C 1.00 1.01 56310G2510\x00 4APHC101',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00AEH MFC  AT USA LHD 1.00 1.00 95740-G2700 201027',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x816H6G5051\x00\x00\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x816U3J9051\x00\x00\xf1\x006U3H1_C2\x00\x006U3J9051\x00\x00HAE0G16NL2\x00\x00\x00\x00',
     ],
   },
   CAR.SONATA: {
@@ -214,45 +253,50 @@ FW_VERSIONS = {
       b'\xf1\x00DN8_ SCC F-CUP      1.00 1.02 99110-L1000         ',
       b'\xf1\x00DN8_ SCC FHCUP      1.00 1.00 99110-L0000         ',
       b'\xf1\x00DN8_ SCC FHCUP      1.00 1.01 99110-L1000         ',
-      b'\xf1\x00DN89110-L0000         \xaa\xaa\xaa\xaa\xaa\xaa\xaa     \xf1\xa01.00\xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00',
-      b'\xf1\x00DN8 1.00 99110-L0000         \xaa\xaa\xaa\xaa\xaa\xaa\xaa     \xf1\xa01.00\xaa\xaa\xaa',
+      b'\xf1\x00DN89110-L0000         \xaa\xaa\xaa\xaa\xaa\xaa\xaa     ',
+      b'\xf1\x00DN8 1.00 99110-L0000         \xaa\xaa\xaa\xaa\xaa\xaa\xaa     ',
+      b'\xf1\x00DN8 1.00 99110-L0000         \xaa\xaa\xaa\xaa\xaa\xaa\xaa\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x8799110L0000\xf1\x00DN8_ SCC FHCUP      1.00 1.00 99110-L0000         ',
     ],
     (Ecu.esp, 0x7d1, None): [
       b'\xf1\x00DN ESC \a 106 \a\x01 58910-L0100',
-      b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300\xf1\xa01.02',
+      b'\xf1\x00DN ESC \x01 102\x19\x04\x13 58910-L1300',
       b'\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300',
       b'\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
       b'\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100',
-      b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300\xf1\xa01.03',
-      b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100\xf1\xa01.06',
-      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
-      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100\xf1\xa01.06',
-      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100\xf1\xa01.04',
+      b'\xf1\x00DN ESC \x08 103\x19\x06\x01 58910-L1300',
+      b'\xf1\x8758910-L0100\xf1\x00DN ESC \a 106 \a\x01 58910-L0100',
+      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 104\x19\x08\x01 58910-L0100',
+      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
+      b'\xf1\x8758910-L0100\xf1\x00DN ESC \x07 104\x19\x08\x01 58910-L0100',
+      b'\xf1\x8758910-L0300\xf1\x00DN ESC \x03 100 \x08\x01 58910-L0300',
       b'\xf1\x00DN ESC \x06 106 \x07\x01 58910-L0100',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x82DNCVN5GMCCXXXF0A',
       b'\xf1\x81HM6M1_0a0_F00',
       b'\xf1\x82DNBVN5GMCCXXXDCA',
       b'\xf1\x82DNBWN5TMDCXXXG2E',
       b'\xf1\x82DNCVN5GMCCXXXG2B',
-      b'\xf1\x87391162M003\xf1\xa0000F',
-      b'\xf1\x87391162M003\xf1\xa00240',
-      b'\xf1\x87391162M013\xf1\xa00240',
+      b'\xf1\x87391162M003',
+      b'\xf1\x87391162M013',
       b'HM6M1_0a0_F00',
       b'HM6M2_0a0_BD0',
+      b'HM6M1_0a0_G20',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
       b'\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101',
       b'\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101',
       b'\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
-      b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101\xf1\xa01.01',
-      b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101\xf1\xa01.01',
-      b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101\xf1\xa01.01',
-      b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103\xf1\xa01.03',
-      b'\xf1\x8756310-L1030\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103\xf1\xa01.03',
-      b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101\xf1\xa01.01',
-      b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101\xf1\xa01.01',
+      b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00DN8 MDPS C 1.00 1.01 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4DNAC101',
+      b'\xf1\x8756310-L0010\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0010 4DNAC101',
+      b'\xf1\x8756310-L0210\xf1\x00DN8 MDPS C 1.00 1.01 56310-L0210 4DNAC101',
+      b'\xf1\x8756310-L1010\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1010 4DNDC103',
+      b'\xf1\x8756310-L1030\xf1\x00DN8 MDPS C 1.00 1.03 56310-L1030 4DNDC103',
+      b'\xf1\x8756310L0010\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0010\x00 4DNAC101',
+      b'\xf1\x8756310L0210\x00\xf1\x00DN8 MDPS C 1.00 1.01 56310L0210\x00 4DNAC101',
+      b'\xf1\x8757700-L0000\xf1\x00DN8 MDPS R 1.00 1.00 57700-L0000 4DNAP100',
     ],
     (Ecu.fwdCamera, 0x7c4, None): [
       b'\xf1\x00DN8 MFC  AT KOR LHD 1.00 1.02 99211-L1000 190422',
@@ -270,6 +314,7 @@ FW_VERSIONS = {
       b'\xf1\x00HT6WA250BLHT6WA910A1SDN8G25NB1\x00\x00\x00\x00\x00\x00\x96\xa1\xf1\x92',
       b'\xf1\x00HT6WA280BLHT6WAD10A1SDN8G25NB2\x00\x00\x00\x00\x00\x00\x08\xc9O:',
       b'\xf1\x00T02601BL  T02730A1  VDN8T25XXX730NS5\xf7_\x92\xf5',
+      b'\xf1\x87954A02N060\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VDN8T25XXX730NS5\xf7_\x92\xf5',
       b'\xf1\x87SALDBA3510954GJ3ww\x87xUUuWx\x88\x87\x88\x87w\x88wvfwfc_\xf9\xff\x98wO\xffl\xe0\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87SALDBA3573534GJ3\x89\x98\x89\x88EUuWgwvwwwwww\x88\x87xTo\xfa\xff\x86f\x7f\xffo\x0e\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87SALDBA3601464GJ3\x88\x88\x88\x88ffvggwvwvw\x87gww\x87wvo\xfb\xff\x98\x88\x7f\xffjJ\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
@@ -281,6 +326,7 @@ FW_VERSIONS = {
       b'\xf1\x87SALDBA6347404GJ3wwwwff\x86hx\x88\x97\x88\x88\x88\x88\x88vfgf\x88?\xfc\xff\x86Uo\xff\xec/\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87SALDBA6901634GJ3UUuWVeVUww\x87wwwwwvUge\x86/\xfb\xff\xbb\x99\x7f\xff]2\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
       b'\xf1\x87SALDBA7077724GJ3\x98\x88\x88\x88ww\x97ygwvwww\x87ww\x88\x87x\x87_\xfd\xff\xba\x99o\xff\x99\x01\xf1\x89HT6WA910A1\xf1\x82SDN8G25NB1\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87SALFBA4195874GJ2EVugvf\x86hgwvwww\x87wgw\x86wc_\xfb\xff\x98\x88\x8f\xff\xe23\xf1\x81U903\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  U903\x00\x00\x00\x00\x00\x00SDN8T16NB0z{\xd4v',
       b'\xf1\x87SAMDBA8054504GJ3gw\x87xffvgffffwwwweUVUf?\xfc\xffvU_\xff\xddl\xf1\x89HT6WAD10A1\xf1\x82SDN8G25NB2\x00\x00\x00\x00\x00\x00',
     ],
   },
@@ -310,21 +356,20 @@ FW_VERSIONS = {
   },
   CAR.SANTA_FE: {
     (Ecu.fwdRadar, 0x7d0, None): [
-      b'\xf1\x00TM__ SCC F-CUP      1.00 1.01 99110-S2000         \xf1\xa01.01',
-      b'\xf1\x00TM__ SCC F-CUP      1.00 1.02 99110-S2000         \xf1\xa01.02',
-      b'\xf1\x00TM__ SCC F-CUP      1.00 1.03 99110-S2000         \xf1\xa01.03',
+      b'\xf1\x00TM__ SCC F-CUP      1.00 1.01 99110-S2000         ',
+      b'\xf1\x00TM__ SCC F-CUP      1.00 1.02 99110-S2000         ',
+      b'\xf1\x00TM__ SCC F-CUP      1.00 1.03 99110-S2000         ',
     ],
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650\xf1\xa01.00',
-      b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650\xf1\xa01.03',
-      b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650\xf1\xa01.04',
-      b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600\xf1\xa01.00',
-      b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600\xf1\xa01.02',
-      b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600\xf1\xa01.03',
-      b'\xf1\x00TM ESC \x02 104\x19\x07\x07 58910-S2600\xf1\xa01.04',
-      b'\xf1\x00TM ESC \x03 103\x18\x11\x07 58910-S2600\xf1\xa01.03',
+      b'\xf1\x00TM ESC \r 100\x18\x031 58910-S2650',
+      b'\xf1\x00TM ESC \r 103\x18\x11\x08 58910-S2650',
+      b'\xf1\x00TM ESC \r 104\x19\a\b 58910-S2650',
+      b'\xf1\x00TM ESC \x02 100\x18\x030 58910-S2600',
+      b'\xf1\x00TM ESC \x02 102\x18\x07\x01 58910-S2600',
+      b'\xf1\x00TM ESC \x02 103\x18\x11\x07 58910-S2600',
+      b'\xf1\x00TM ESC \x02 104\x19\x07\x07 58910-S2600',
+      b'\xf1\x00TM ESC \x03 103\x18\x11\x07 58910-S2600',
       b'\xf1\x00TM ESC \x0c 103\x18\x11\x08 58910-S2650',
-      b'\xf1\x00TM ESC \x0c 103\x18\x11\x08 58910-S2650\xf1\xa01.03',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81606EA051\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -364,10 +409,38 @@ FW_VERSIONS = {
       b'\xf1\x87SBLWAA4899564GG0VfvgUU\x85Xx\x88\x87\x88vfgf\x87wxwvO\xfb\xff\x97f\xb1\xffSB\xf1\x816W3E1051\x00\x00\xf1\x006W351_C2\x00\x006W3E1051\x00\x00TTM2G24NS7\x00\x00\x00\x00',
     ],
   },
+  CAR.SANTA_FE_2022: {
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
+      b'\xf1\x8799110S1500\xf1\x00TM__ SCC F-CUP      1.00 1.00 99110-S1500         ',
+    ],
+    (Ecu.esp, 0x7d1, None): [
+      b'\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
+      b'\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
+      b'\xf1\x8758910-S2DA0\xf1\x00TM ESC \x03 101 \x08\x02 58910-S2DA0',
+      b'\xf1\x8758910-S2GA0\xf1\x00TM ESC \x02 101 \x08\x04 58910-S2GA0',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x82TMBZN5TMD3XXXG2E',
+      b'\xf1\x82TACVN5GSI3XXXH0A',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x00TM  MDPS C 1.00 1.02 56370-S2AA0 0B19',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00TMA MFC  AT MEX LHD 1.00 1.01 99211-S2500 210205',
+      b'\xf1\x00TMA MFC  AT USA LHD 1.00 1.00 99211-S2500 200720',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
+      b'\xf1\x87SDMXCA8653204GN1EVugEUuWwwwwww\x87wwwwwv/\xfb\xff\xa8\x88\x9f\xff\xa5\x9c\xf1\x89HT6WAD00A1\xf1\x82STM4G25NH1\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87954A02N250\x00\x00\x00\x00\x00\xf1\x81T02730A1  \xf1\x00T02601BL  T02730A1  VTMPT25XXX730NS2\xa6\x06\x88\xf7',
+    ],
+  },
   CAR.KIA_STINGER: {
     (Ecu.fwdRadar, 0x7d0, None): [
-      b'\xf1\x00CK__ SCC F_CUP      1.00 1.01 96400-J5100         \xf1\xa01.01',
-      b'\xf1\x00CK__ SCC F_CUP      1.00 1.03 96400-J5100         \xf1\xa01.03',
+      b'\xf1\x00CK__ SCC F_CUP      1.00 1.01 96400-J5100         ',
+      b'\xf1\x00CK__ SCC F_CUP      1.00 1.03 96400-J5100         ',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81606DE051\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -397,23 +470,21 @@ FW_VERSIONS = {
   },
   CAR.PALISADE: {
     (Ecu.fwdRadar, 0x7d0, None): [
-      b'\xf1\000LX2_ SCC F-CUP      1.00 1.05 99110-S8100         \xf1\xa01.05',
-      b'\xf1\x00LX2 SCC FHCUP      1.00 1.04 99110-S8100         \xf1\xa01.04',
-      b'\xf1\x00LX2_ SCC FHCUP      1.00 1.04 99110-S8100         \xf1\xa01.04',
-      b'\xf1\x00LX2_ SCC FHCUP      1.00 1.05 99110-S8100         \xf1\xa01.05',
+      b'\xf1\000LX2_ SCC F-CUP      1.00 1.05 99110-S8100         ',
+      b'\xf1\x00LX2 SCC FHCUP      1.00 1.04 99110-S8100         ',
+      b'\xf1\x00LX2_ SCC FHCUP      1.00 1.04 99110-S8100         ',
+      b'\xf1\x00LX2_ SCC FHCUP      1.00 1.05 99110-S8100         ',
     ],
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x00LX ESC \v 102\x19\x05\a 58910-S8330\xf1\xa01.02',
-      b'\xf1\x00LX ESC \v 103\x19\t\x10 58910-S8360\xf1\xa01.03',
-      b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360\xf1\xa01.03',
-      b'\xf1\x00LX ESC \x01 103\x31\t\020 58910-S8360\xf1\xa01.03',
-      b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330\xf1\xa01.01',
+      b'\xf1\x00LX ESC \x01 103\x19\t\x10 58910-S8360',
+      b'\xf1\x00LX ESC \x01 103\x31\t\020 58910-S8360',
+      b'\xf1\x00LX ESC \x0b 101\x19\x03\x17 58910-S8330',
       b'\xf1\x00LX ESC \x0b 102\x19\x05\x07 58910-S8330',
-      b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330\xf1\xa01.03',
+      b'\xf1\x00LX ESC \x0b 103\x19\t\x07 58910-S8330',
       b'\xf1\x00LX ESC \x0b 103\x19\t\x10 58910-S8360',
-      b'\xf1\x00LX ESC \x0b 104 \x10\x16 58910-S8360\xf1\xa01.04',
-      b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360\xf1\xa01.00',
-      b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360\xf1\xa01.01',
+      b'\xf1\x00LX ESC \x0b 104 \x10\x16 58910-S8360',
+      b'\xf1\x00ON ESC \x0b 100\x18\x12\x18 58910-S9360',
+      b'\xf1\x00ON ESC \x0b 101\x19\t\x08 58910-S9360',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81640J0051\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -480,15 +551,15 @@ FW_VERSIONS = {
     ],
   },
   CAR.GENESIS_G70: {
-    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         \xf1\xa01.02', ],
+    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00IK__ SCC F-CUP      1.00 1.02 96400-G9100         ', ],
     (Ecu.engine, 0x7e0, None): [b'\xf1\x81640F0051\x00\x00\x00\x00\x00\x00\x00\x00', ],
     (Ecu.eps, 0x7d4, None): [b'\xf1\x00IK  MDPS R 1.00 1.06 57700-G9420 4I4VL106', ],
     (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00IK  MFC  AT USA LHD 1.00 1.01 95740-G9000 170920', ],
     (Ecu.transmission, 0x7e1, None): [b'\xf1\x87VDJLT17895112DN4\x88fVf\x99\x88\x88\x88\x87fVe\x88vhwwUFU\x97eFex\x99\xff\xb7\x82\xf1\x81E25\x00\x00\x00\x00\x00\x00\x00\xf1\x00bcsh8p54  E25\x00\x00\x00\x00\x00\x00\x00SIK0T33NB2\x11\x1am\xda', ],
   },
   CAR.KONA: {
-    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP      1.00 1.00 95655-J9200         \xf1\xa01.00', ],
-    (Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00\xf1\xa01.05', ],
+    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00OS__ SCC F-CUP      1.00 1.00 95655-J9200         ', ],
+    (Ecu.esp, 0x7d1, None): [b'\xf1\x816V5RAK00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00', ],
     (Ecu.engine, 0x7e0, None): [b'"\x01TOS-0NU06F301J02', ],
     (Ecu.eps, 0x7d4, None): [b'\xf1\x00OS  MDPS C 1.00 1.05 56310J9030\x00 4OSDC105', ],
     (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00OS9 LKAS AT USA LHD 1.00 1.00 95740-J9300 g21', ],
@@ -510,25 +581,49 @@ FW_VERSIONS = {
       b'\xf1\x00BD  LKAS AT USA LHD 1.00 1.04 95740-M6000 J33',
     ],
     (Ecu.fwdRadar, 0x7D0, None): [
-      b'\xf1\x00BD__ SCC H-CUP      1.00 1.02 99110-M6000         \xf1\xa01.02',
+      b'\xf1\x00BD__ SCC H-CUP      1.00 1.02 99110-M6000         ',
     ],
     (Ecu.engine, 0x7e0, None): [
       b'\x01TBDM1NU06F200H01',
     ],
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00\xf1\xa01.04',
+      b'\xf1\x816VGRAH00018.ELF\xf1\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b"\xf1\x816U2VC051\x00\x00\xf1\x006U2V0_C2\x00\x006U2VC051\x00\x00DBD0T16SS0\xcf\x1e'\xc3",
     ],
   },
+  CAR.KIA_K5_2021: {
+    (Ecu.fwdRadar, 0x7D0, None): [
+      b'\xf1\000DL3_ SCC FHCUP      1.00 1.03 99110-L2000         ',
+      b'\xf1\x8799110L2000\xf1\000DL3_ SCC FHCUP      1.00 1.03 99110-L2000         ',
+    ],
+    (Ecu.eps, 0x7D4, None): [
+      b'\xf1\x8756310-L3110\xf1\000DL3 MDPS C 1.00 1.01 56310-L3110 4DLAC101',
+    ],
+    (Ecu.fwdCamera, 0x7C4, None): [
+      b'\xf1\000DL3 MFC  AT USA LHD 1.00 1.03 99210-L3000 200915',
+    ],
+    (Ecu.esp, 0x7D1, None): [
+      b'\xf1\000DL ESC \006 101 \004\002 58910-L3200',
+      b'\xf1\x8758910-L3200\xf1\000DL ESC \006 101 \004\002 58910-L3200',
+    ],
+    (Ecu.engine, 0x7E0, None): [
+      b'\xf1\x87391212MKT0\xf1\xa00240',
+      b'\xf1\x87391212MKT0',
+    ],
+    (Ecu.transmission, 0x7E1, None): [
+      b'\xf1\000bcsh8p54  U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8',
+      b'\xf1\x87SALFEA5652514GK2UUeV\x88\x87\x88xxwg\x87ww\x87wwfwvd/\xfb\xffvU_\xff\x93\xd3\xf1\x81U913\000\000\000\000\000\000\xf1\000bcsh8p54  U913\000\000\000\000\000\000TDL2T16NB1ia\v\xb8',
+    ],
+  },
   CAR.KONA_EV: {
     (Ecu.esp, 0x7D1, None): [
-      b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000\xf1\xa01.05',
-      b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000\xf1\xa02.12',
-      b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000\xf1\xa02.12',
-      b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000\xf1\xa02.10',
-      b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000\xf1\xa02.12',
+      b'\xf1\x00OS IEB \r 105\x18\t\x18 58520-K4000',
+      b'\xf1\x00OS IEB \x01 212 \x11\x13 58520-K4000',
+      b'\xf1\x00OS IEB \x02 212 \x11\x13 58520-K4000',
+      b'\xf1\x00OS IEB \x03 210 \x02\x14 58520-K4000',
+      b'\xf1\x00OS IEB \x03 212 \x11\x13 58520-K4000',
     ],
     (Ecu.fwdCamera, 0x7C4, None): [
       b'\xf1\x00OE2 LKAS AT EUR LHD 1.00 1.00 95740-K4200 200',
@@ -543,27 +638,23 @@ FW_VERSIONS = {
       b'\xf1\x00OS  MDPS C 1.00 1.04 56310K4050\x00 4OEDC104',
     ],
     (Ecu.fwdRadar, 0x7D0, None): [
-      b'\xf1\x00OSev SCC F-CUP      1.00 1.00 99110-K4000         \xf1\xa01.00',
-      b'\xf1\x00OSev SCC F-CUP      1.00 1.00 99110-K4100         \xf1\xa01.00',
-      b'\xf1\x00OSev SCC F-CUP      1.00 1.01 99110-K4000         \xf1\xa01.01',
-      b'\xf1\x00OSev SCC FNCUP      1.00 1.01 99110-K4000         \xf1\xa01.01',
+      b'\xf1\x00OSev SCC F-CUP      1.00 1.00 99110-K4000         ',
+      b'\xf1\x00OSev SCC F-CUP      1.00 1.00 99110-K4100         ',
+      b'\xf1\x00OSev SCC F-CUP      1.00 1.01 99110-K4000         ',
+      b'\xf1\x00OSev SCC FNCUP      1.00 1.01 99110-K4000         ',
     ],
   },
   CAR.KIA_NIRO_EV: {
     (Ecu.fwdRadar, 0x7D0, None): [
       b'\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         ',
-      b'\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         \xf1\xa01.00',
-      b'\xf1\x00DEev SCC F-CUP      1.00 1.03 96400-Q4100         \xf1\xa01.03',
-      b'\xf1\x00OSev SCC F-CUP      1.00 1.01 99110-K4000         \xf1\xa01.01',
-      b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         \xf1\xa01.00',
-      b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4100         \xf1\xa01.00',
-      b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP      1.00 1.00 99110-Q4500         \xf1\xa01.00',
+      b'\xf1\x00DEev SCC F-CUP      1.00 1.03 96400-Q4100         ',
+      b'\xf1\x00OSev SCC F-CUP      1.00 1.01 99110-K4000         ',
+      b'\xf1\x8799110Q4000\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4000         ',
+      b'\xf1\x8799110Q4100\xf1\x00DEev SCC F-CUP      1.00 1.00 99110-Q4100         ',
+      b'\xf1\x8799110Q4500\xf1\000DEev SCC F-CUP      1.00 1.00 99110-Q4500         ',
     ],
     (Ecu.esp, 0x7D1, None): [
-      b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000\xf1\xa02.12',
       b'\xf1\x00OS IEB \r 212 \x11\x13 58520-K4000',
-      b'\xf1\xa01.06',
-      b'\xf1\xa01.07',
     ],
     (Ecu.eps, 0x7D4, None): [
       b'\xf1\x00DE  MDPS C 1.00 1.05 56310Q4000\x00 4DEEC105',
@@ -615,10 +706,10 @@ FW_VERSIONS = {
     ],
   },
   CAR.KIA_SELTOS: {
-    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP      1.01 1.05 99110-Q5100         \xf1\xa01.05',],
+    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x8799110Q5100\xf1\000SP2_ SCC FHCUP      1.01 1.05 99110-Q5100         ',],
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450\xf1\xa01.01',
-      b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450\xf1\xa01.01',
+      b'\xf1\x8758910-Q5450\xf1\000SP ESC \a 101\031\t\005 58910-Q5450',
+      b'\xf1\x8758910-Q5450\xf1\000SP ESC \t 101\031\t\005 58910-Q5450',
      ],
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x81616D2051\000\000\000\000\000\000\000\000',
@@ -652,22 +743,36 @@ FW_VERSIONS = {
     (Ecu.transmission, 0x7e1, None): [b'\xf1\x816U2V8051\x00\x00\xf1\x006U2V0_C2\x00\x006U2V8051\x00\x00DJF0T16NL0\t\xd2GW'],
   },
   CAR.ELANTRA_2021: {
-    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         '],
-    (Ecu.eps, 0x7d4, None): [
-      b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106\xf1\xa01.06',
-      b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106\xf1\xa01.06',
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\x00CN7_ SCC FHCUP      1.00 1.01 99110-AA000         ',
+      b'\xf1\x00CN7_ SCC F-CUP      1.00 1.01 99110-AA000         ',
+    ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x87\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\xf1\x00CN7 MDPS C 1.00 1.06 \x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00 4CNDC106',
+      b'\xf1\x8756310AA050\x00\xf1\x00CN7 MDPS C 1.00 1.06 56310AA050\x00 4CNDC106',
+      b'\xf1\x8756310/AA070\xf1\x00CN7 MDPS C 1.00 1.06 56310/AA070 4CNDC106\xf1\xa01.06',
+    ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00CN7 MFC  AT USA LHD 1.00 1.00 99210-AB000 200819',
+      b'\xf1\x00CN7 MFC  AT USA LHD 1.00 1.03 99210-AA000 200819',
     ],
-    (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\x00CN7 MFC  AT USA LHD 1.00 1.00 99210-AB000 200819'],
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800\xf1\xa01.01',
+      b'\xf1\x8758910-AB800\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
       b'\xf1\x00CN ESC \t 101 \x10\x03 58910-AB800',
+      b'\xf1\x8758910-AA800\xf1\x00CN ESC \t 104 \x08\x03 58910-AA800\xf1\xa01.04',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\xf1\x00HT6WA280BLHT6VA640A1CCN0N20NS5\x00\x00\x00\x00\x00\x00\xe8\xba\xce\xfa',
       b'\xf1\x87CXMQFM2135005JB2E\xb9\x89\x98W\xa9y\x97h\xa9\x98\x99wxvwh\x87\177\xffx\xff\xff\xff,,\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87CXMQFM1916035JB2\x88vvgg\x87Wuwgev\xa9\x98\x88\x98h\x99\x9f\xffh\xff\xff\xff\xa5\xee\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
+      b'\xf1\x87CXLQF40189012JL2f\x88\x86\x88\x88vUex\xb8\x88\x88\x88\x87\x88\x89fh?\xffz\xff\xff\xff\x08z\xf1\x89HT6VA640A1\xf1\x82CCN0N20NS5\x00\x00\x00\x00\x00\x00',
+    ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x82CNCWD0AMFCXCSFFA',
+      b'\xf1\x82CNCWD0AMFCXCSFFB',
+      b'\xf1\x82CNCVD0AMFCXCSFFB',
     ],
-    (Ecu.engine, 0x7e0, None): [b'\xf1\x82CNCWD0AMFCXCSFFA'],
   },
   CAR.ELANTRA_HEV_2021: {
     (Ecu.fwdCamera, 0x7c4, None) : [
@@ -677,9 +782,11 @@ FW_VERSIONS = {
       b'\xf1\000CNhe SCC FHCUP      1.00 1.01 99110-BY000         '
     ],
     (Ecu.eps, 0x7d4, None) :[
-      b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102\xf1\xa01.02'
+      b'\xf1\x8756310/BY050\xf1\000CN7 MDPS C 1.00 1.02 56310/BY050 4CNHC102'
     ],
     (Ecu.transmission, 0x7e1, None) :[
+      b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
+      b'\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000',
       b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\xb9?A\xaa',
       b'\xf1\x816U3K3051\000\000\xf1\0006U3L0_C2\000\0006U3K3051\000\000HCN0G16NS0\000\000\000\000'
     ],
@@ -689,10 +796,10 @@ FW_VERSIONS = {
   },
   CAR.KONA_HEV: {
     (Ecu.esp, 0x7d1, None): [
-      b'\xf1\x00OS IEB \x01 104 \x11  58520-CM000\xf1\xa01.04',
+      b'\xf1\x00OS IEB \x01 104 \x11  58520-CM000',
     ],
     (Ecu.fwdRadar, 0x7d0, None): [
-      b'\xf1\x00OShe SCC FNCUP      1.00 1.01 99110-CM000         \xf1\xa01.01',
+      b'\xf1\x00OShe SCC FNCUP      1.00 1.01 99110-CM000         ',
     ],
     (Ecu.eps, 0x7d4, None): [
       b'\xf1\x00OS  MDPS C 1.00 1.00 56310CM030\x00 4OHDC100',
@@ -708,16 +815,38 @@ FW_VERSIONS = {
     ]
   },
   CAR.SONATA_HYBRID: {
-    (Ecu.fwdRadar, 0x7d0, None): [b'\xf1\000DNhe SCC FHCUP      1.00 1.02 99110-L5000         ',],
-    (Ecu.eps, 0x7d4, None): [b'\xf1\x8756310-L5500\xf1\000DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102\xf1\xa01.02',],
-    (Ecu.fwdCamera, 0x7c4, None): [b'\xf1\000DN8HMFC  AT USA LHD 1.00 1.04 99211-L1000 191016',],
-    (Ecu.transmission, 0x7e1, None): [b'\xf1\000PSBG2323  E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',],
-    (Ecu.engine, 0x7e0, None): [b'\xf1\x87391162J012\xf1\xa0000R',],
+    (Ecu.fwdRadar, 0x7d0, None): [
+      b'\xf1\000DNhe SCC FHCUP      1.00 1.02 99110-L5000         ',
+      b'\xf1\x8799110L5000\xf1\000DNhe SCC FHCUP      1.00 1.02 99110-L5000         ',
+      b'\xf1\000DNhe SCC F-CUP      1.00 1.02 99110-L5000         ',
+      b'\xf1\x8799110L5000\xf1\000DNhe SCC F-CUP      1.00 1.02 99110-L5000         ',
+      ],
+    (Ecu.eps, 0x7d4, None): [
+      b'\xf1\x8756310-L5500\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5500 4DNHC102',
+      b'\xf1\x8756310-L5450\xf1\x00DN8 MDPS C 1.00 1.02 56310-L5450 4DNHC102',
+      b'\xf1\x8756310-L5450\xf1\000DN8 MDPS C 1.00 1.03 56310-L5450 4DNHC103',
+      ],
+    (Ecu.fwdCamera, 0x7c4, None): [
+      b'\xf1\x00DN8HMFC  AT USA LHD 1.00 1.04 99211-L1000 191016',
+      b'\xf1\x00DN8HMFC  AT USA LHD 1.00 1.05 99211-L1000 201109',
+      b'\xf1\000DN8HMFC  AT USA LHD 1.00 1.06 99211-L1000 210325',
+      ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\000PSBG2333  E14\x00\x00\x00\x00\x00\x00\x00TDN2H20SA6N\xc2\xeeW',
+      b'\xf1\x87959102T250\000\000\000\000\000\xf1\x81E09\000\000\000\000\000\000\000\xf1\000PSBG2323  E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',
+      b'\xf1\000PSBG2323  E09\000\000\000\000\000\000\000TDN2H20SA5\x97R\x88\x9e',
+      b'\xf1\000PSBG2333  E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
+      b'\xf1\x87PCU\000\000\000\000\000\000\000\000\000\xf1\x81E16\000\000\000\000\000\000\000\xf1\000PSBG2333  E16\000\000\000\000\000\000\000TDN2H20SA7\0323\xf9\xab',
+      ],
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x87391162J012',
+      b'\xf1\x87391162J013',
+      ],
   },
 }
 
 CHECKSUM = {
-  "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID],
+  "crc8": [CAR.SANTA_FE, CAR.SONATA, CAR.PALISADE, CAR.KIA_SELTOS, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.SANTA_FE_2022, CAR.KIA_K5_2021],
   "6B": [CAR.KIA_SORENTO, CAR.HYUNDAI_GENESIS],
 }
 
@@ -725,15 +854,17 @@ FEATURES = {
   # which message has the gear
   "use_cluster_gears": set([CAR.ELANTRA, CAR.ELANTRA_GT_I30, CAR.KONA]),
   "use_tcu_gears": set([CAR.KIA_OPTIMA, CAR.SONATA_LF, CAR.VELOSTER]),
-  "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV]),
+  "use_elect_gears": set([CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.KIA_OPTIMA_H, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021,CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ_HEV_2022]),
 
   # these cars use the FCA11 message for the AEB and FCW signals, all others use SCC12
-  "use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV]),
+  "use_fca": set([CAR.SONATA, CAR.SONATA_HYBRID, CAR.ELANTRA, CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.ELANTRA_GT_I30, CAR.KIA_STINGER, CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KONA_EV, CAR.KIA_FORTE, CAR.KIA_NIRO_EV, CAR.PALISADE, CAR.GENESIS_G70, CAR.KONA, CAR.SANTA_FE, CAR.KIA_SELTOS, CAR.KONA_HEV, CAR.SANTA_FE_2022, CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022]),
 }
 
-HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV])  # these cars use a different gas signal
-EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.IONIQ, CAR.KONA_EV, CAR.KIA_NIRO_EV])
+HYBRID_CAR = set([CAR.IONIQ_PHEV, CAR.ELANTRA_HEV_2021, CAR.KIA_NIRO_HEV, CAR.KIA_NIRO_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.IONIQ, CAR.IONIQ_HEV_2022])  # these cars use a different gas signal
+EV_CAR = set([CAR.IONIQ_EV_2020, CAR.IONIQ_EV_LTD, CAR.KONA_EV, CAR.KIA_NIRO_EV])
 
+# If 0x500 is present on bus 1 it probably has a Mando radar outputting radar points.
+# If no points are outputted by default it might be possible to turn it on using  selfdrive/debug/hyundai_enable_radar_points.py
 DBC = {
   CAR.ELANTRA: dbc_dict('hyundai_kia_generic', None),
   CAR.ELANTRA_2021: dbc_dict('hyundai_kia_generic', None),
@@ -745,27 +876,30 @@ DBC = {
   CAR.HYUNDAI_GENESIS: dbc_dict('hyundai_kia_generic', None),
   CAR.IONIQ_PHEV: dbc_dict('hyundai_kia_generic', None),
   CAR.IONIQ_EV_2020: dbc_dict('hyundai_kia_generic', None),
-  CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', None),
+  CAR.IONIQ_EV_LTD: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.IONIQ: dbc_dict('hyundai_kia_generic', None),
+  CAR.IONIQ_HEV_2022: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_FORTE: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_K5_2021: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_NIRO_EV: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_NIRO_HEV: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.KIA_NIRO_HEV_2021: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_OPTIMA_H: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_SELTOS: dbc_dict('hyundai_kia_generic', None),
-  CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None),
+  CAR.KIA_SORENTO: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
   CAR.KIA_STINGER: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA_EV: dbc_dict('hyundai_kia_generic', None),
   CAR.KONA_HEV: dbc_dict('hyundai_kia_generic', None),
-  CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', None),
-  CAR.SONATA: dbc_dict('hyundai_kia_generic', None),
-  CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None),
-  CAR.PALISADE: dbc_dict('hyundai_kia_generic', None),
+  CAR.SANTA_FE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
+  CAR.SANTA_FE_2022: dbc_dict('hyundai_kia_generic', None),
+  CAR.SONATA: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
+  CAR.SONATA_LF: dbc_dict('hyundai_kia_generic', None), # Has 0x5XX messages, but different format
+  CAR.PALISADE: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
   CAR.VELOSTER: dbc_dict('hyundai_kia_generic', None),
   CAR.KIA_CEED: dbc_dict('hyundai_kia_generic', None),
-  CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', None),
+  CAR.SONATA_HYBRID: dbc_dict('hyundai_kia_generic', 'hyundai_kia_mando_front_radar'),
 }
 
 STEER_THRESHOLD = 150
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index f74fe06e0..ada906367 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -48,10 +48,6 @@ class CarInterfaceBase():
   def get_pid_accel_limits(CP, current_speed, cruise_speed):
     return ACCEL_MIN, ACCEL_MAX
 
-  @staticmethod
-  def calc_accel_override(a_ego, a_target, v_ego, v_target):
-    return 1.
-
   @staticmethod
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     raise NotImplementedError
@@ -60,6 +56,16 @@ class CarInterfaceBase():
   def init(CP, logcan, sendcan):
     pass
 
+  @staticmethod
+  def get_steer_feedforward_default(desired_angle, v_ego):
+    # Proportional to realigning tire momentum: lateral acceleration.
+    # TODO: something with lateralPlan.curvatureRates
+    return desired_angle * (v_ego**2)
+
+  @classmethod
+  def get_steer_feedforward_function(cls):
+    return cls.get_steer_feedforward_default
+
   # returns a set of default params to avoid repetition in car specific params
   @staticmethod
   def get_std_params(candidate, fingerprint):
@@ -76,10 +82,13 @@ class CarInterfaceBase():
     ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
     ret.steerRatioRear = 0.  # no rear steering, at least on the listed cars aboveA
     ret.openpilotLongitudinalControl = False
-    ret.startAccel = 0.0
     ret.minSpeedCan = 0.3
-    ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
+    ret.startAccel = -0.8
+    ret.stopAccel = -2.0
     ret.startingAccelRate = 3.2 # brake_travel/s while releasing on restart
+    ret.stoppingDecelRate = 0.8 # brake_travel/s while trying to stop
+    ret.vEgoStopping = 0.5
+    ret.vEgoStarting = 0.5
     ret.stoppingControl = True
     ret.longitudinalTuning.deadzoneBP = [0.]
     ret.longitudinalTuning.deadzoneV = [0.]
@@ -87,7 +96,8 @@ class CarInterfaceBase():
     ret.longitudinalTuning.kpV = [1.]
     ret.longitudinalTuning.kiBP = [0.]
     ret.longitudinalTuning.kiV = [1.]
-    ret.longitudinalActuatorDelay = 0.15
+    ret.longitudinalActuatorDelayLowerBound = 0.15
+    ret.longitudinalActuatorDelayUpperBound = 0.15
     return ret
 
   # returns a car.CarState, pass in car.CarControl
@@ -134,7 +144,8 @@ class CarInterfaceBase():
     elif cs_out.steerWarning:
       # only escalate to the harsher alert after the condition has
       # persisted for 0.5s and we're certain that the user isn't overriding
-      if self.steering_unpressed > int(0.5/DT_CTRL) and self.steer_warning > int(0.5/DT_CTRL):
+      if not cs_out.standstill and self.steering_unpressed > int(0.5 / DT_CTRL) and \
+         self.steer_warning > int(0.5 / DT_CTRL):
         events.add(EventName.steerTempUnavailable)
       else:
         events.add(EventName.steerTempUnavailableSilent)
diff --git a/selfdrive/car/isotp_parallel_query.py b/selfdrive/car/isotp_parallel_query.py
index f95152258..1209a8f1a 100644
--- a/selfdrive/car/isotp_parallel_query.py
+++ b/selfdrive/car/isotp_parallel_query.py
@@ -71,7 +71,10 @@ class IsoTpParallelQuery:
     messaging.drain_sock(self.logcan)
     self.msg_buffer = defaultdict(list)
 
-  def get_data(self, timeout):
+  def get_data(self, timeout, total_timeout=None):
+    if total_timeout is None:
+      total_timeout = 10 * timeout
+
     self._drain_rx()
 
     # Create message objects
@@ -96,7 +99,8 @@ class IsoTpParallelQuery:
       request_done[tx_addr] = False
 
     results = {}
-    start_time = time.time()
+    start_time = time.monotonic()
+    last_response_time = start_time
     while True:
       self.rx()
 
@@ -104,7 +108,12 @@ class IsoTpParallelQuery:
         break
 
       for tx_addr, msg in msgs.items():
-        dat: Optional[bytes] = msg.recv()
+        try:
+          dat: Optional[bytes] = msg.recv()
+        except Exception:
+          cloudlog.exception("Error processing UDS response")
+          request_done[tx_addr] = True
+          continue
 
         if not dat:
           continue
@@ -114,6 +123,7 @@ class IsoTpParallelQuery:
         response_valid = dat[:len(expected_response)] == expected_response
 
         if response_valid:
+          last_response_time = time.monotonic()
           if counter + 1 < len(self.request):
             msg.send(self.request[counter + 1])
             request_counter[tx_addr] += 1
@@ -124,7 +134,15 @@ class IsoTpParallelQuery:
           request_done[tx_addr] = True
           cloudlog.warning(f"iso-tp query bad response: 0x{dat.hex()}")
 
-      if time.time() - start_time > timeout:
+      cur_time = time.monotonic()
+      if cur_time - last_response_time > timeout:
+        for tx_addr in msgs:
+          if (request_counter[tx_addr] > 0) and (not request_done[tx_addr]):
+            cloudlog.warning(f"iso-tp query timeout after receiving response: {tx_addr}")
+        break
+
+      if cur_time - start_time > total_timeout:
+        cloudlog.warning("iso-tp query timeout while receiving data")
         break
 
     return results
diff --git a/selfdrive/car/mazda/carcontroller.py b/selfdrive/car/mazda/carcontroller.py
index 5d9e9c9bb..bda5378dd 100644
--- a/selfdrive/car/mazda/carcontroller.py
+++ b/selfdrive/car/mazda/carcontroller.py
@@ -8,6 +8,7 @@ class CarController():
     self.apply_steer_last = 0
     self.packer = CANPacker(dbc_name)
     self.steer_rate_limited = False
+    self.brake_counter = 0
 
   def update(self, enabled, CS, frame, actuators):
     """ Controls thread """
@@ -31,10 +32,17 @@ class CarController():
     else:
       apply_steer = 0
       self.steer_rate_limited = False
-      if CS.out.cruiseState.enabled and frame % 20 == 0:
-        # Cancel Stock ACC if it's enabled while OP is disengaged
-        # Send at a rate of 5hz until we sync with stock ACC state
-        can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL))
+      if CS.out.cruiseState.enabled:
+        # if brake is pressed, let us wait >20ms before trying to disable crz to avoid
+        # a race condition with the stock system, where the second cancel from openpilot
+        # will disable the crz 'main on'
+        self.brake_counter = self.brake_counter + 1
+        if frame % 20 == 0 and not (CS.out.brakePressed and self.brake_counter < 3):
+          # Cancel Stock ACC if it's enabled while OP is disengaged
+          # Send at a rate of 5hz until we sync with stock ACC state
+          can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, Buttons.CANCEL))
+      else:
+        self.brake_counter = 0
 
     self.apply_steer_last = apply_steer
 
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index fb720ad8f..a96c68b0e 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -64,15 +64,9 @@ class CarState(CarStateBase):
     elif speed_kph < LKAS_LIMITS.DISABLE_SPEED:
       self.lkas_allowed = False
 
-    # if any of the cruize buttons is pressed force state update
-    if any([cp.vl["CRZ_BTNS"]["RES"],
-                cp.vl["CRZ_BTNS"]["SET_P"],
-                cp.vl["CRZ_BTNS"]["SET_M"]]):
-      self.cruise_speed = ret.vEgoRaw
-
-    ret.cruiseState.available = True
+    ret.cruiseState.available = cp.vl["CRZ_CTRL"]["CRZ_AVAILABLE"] == 1
     ret.cruiseState.enabled = cp.vl["CRZ_CTRL"]["CRZ_ACTIVE"] == 1
-    ret.cruiseState.speed = self.cruise_speed
+    ret.cruiseState.speed = cp.vl["CRZ_EVENTS"]["CRZ_SPEED"] * CV.KPH_TO_MS
 
     if ret.cruiseState.enabled:
       if not self.lkas_allowed:
@@ -84,8 +78,9 @@ class CarState(CarStateBase):
         self.low_speed_lockout = False
         self.low_speed_alert = False
 
-    # On if no driver torque the last 5 seconds
-    ret.steerWarning = cp.vl["STEER_RATE"]["HANDS_OFF_5_SECONDS"] == 1
+    # Check if LKAS is disabled due to lack of driver torque when all other states indicate
+    # it should be enabled (steer lockout)
+    ret.steerWarning = self.lkas_allowed and (cp.vl["STEER_RATE"]["LKAS_BLOCK"] == 1)
 
     self.acc_active_last = ret.cruiseState.enabled
 
@@ -126,6 +121,8 @@ class CarState(CarStateBase):
         ("LKAS_TRACK_STATE", "STEER_RATE", 0),
         ("HANDS_OFF_5_SECONDS", "STEER_RATE", 0),
         ("CRZ_ACTIVE", "CRZ_CTRL", 0),
+        ("CRZ_AVAILABLE", "CRZ_CTRL", 0),
+        ("CRZ_SPEED", "CRZ_EVENTS", 0),
         ("STANDSTILL", "PEDALS", 0),
         ("BRAKE_ON", "PEDALS", 0),
         ("BRAKE_PRESSURE", "BRAKE", 0),
@@ -148,6 +145,7 @@ class CarState(CarStateBase):
       checks += [
         ("ENGINE_DATA", 100),
         ("CRZ_CTRL", 50),
+        ("CRZ_EVENTS", 50),
         ("CRZ_BTNS", 10),
         ("PEDALS", 50),
         ("BRAKE", 50),
diff --git a/selfdrive/car/mazda/interface.py b/selfdrive/car/mazda/interface.py
index dfc1c12bf..5472a0297 100755
--- a/selfdrive/car/mazda/interface.py
+++ b/selfdrive/car/mazda/interface.py
@@ -2,7 +2,7 @@
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.car.mazda.values import CAR, LKAS_LIMITS
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 ButtonType = car.CarState.ButtonEvent.Type
@@ -19,10 +19,9 @@ class CarInterface(CarInterfaceBase):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "mazda"
-    ret.safetyModel = car.CarParams.SafetyModel.mazda
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.mazda)]
     ret.radarOffCan = True
 
-    ret.communityFeature = True
     ret.dashcamOnly = True
 
     ret.steerActuatorDelay = 0.1
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index 2620a35a5..4e4ecd341 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -204,6 +204,7 @@ FW_VERSIONS = {
     ],
     (Ecu.fwdRadar, 0x764, None): [
       b'K131-67XK2-E\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'K131-67XK2-F\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.esp, 0x760, None): [
       b'TA0B-437K2-C\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -211,6 +212,7 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x706, None): [
       b'GSH7-67XK2-M\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'GSH7-67XK2-N\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'GSH7-67XK2-P\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'PXM4-21PS1-B\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py
index 604851cb9..999e735c7 100755
--- a/selfdrive/car/mock/interface.py
+++ b/selfdrive/car/mock/interface.py
@@ -4,7 +4,7 @@ from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.swaglog import cloudlog
 import cereal.messaging as messaging
-from selfdrive.car import gen_empty_fingerprint
+from selfdrive.car import gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 # mocked car interface to work with chffrplus
@@ -37,7 +37,7 @@ class CarInterface(CarInterfaceBase):
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "mock"
-    ret.safetyModel = car.CarParams.SafetyModel.noOutput
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.noOutput)]
     ret.mass = 1700.
     ret.rotationalInertia = 2500.
     ret.wheelbase = 2.70
diff --git a/selfdrive/car/nissan/interface.py b/selfdrive/car/nissan/interface.py
index 3edcbd0f8..18ae885f8 100644
--- a/selfdrive/car/nissan/interface.py
+++ b/selfdrive/car/nissan/interface.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.car.nissan.values import CAR
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 class CarInterface(CarInterfaceBase):
@@ -14,10 +14,7 @@ class CarInterface(CarInterfaceBase):
 
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "nissan"
-    ret.safetyModel = car.CarParams.SafetyModel.nissan
-
-    # Nissan port is a community feature, since we don't own one to test
-    ret.communityFeature = True
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.nissan)]
 
     ret.steerLimitAlert = False
     ret.steerRateCost = 0.5
@@ -36,7 +33,7 @@ class CarInterface(CarInterfaceBase):
       ret.steerRatio = 17
     elif candidate == CAR.ALTIMA:
       # Altima has EPS on C-CAN unlike the others that have it on V-CAN
-      ret.safetyParam = 1 # EPS is on alternate bus
+      ret.safetyConfigs[0].safetyParam = 1 # EPS is on alternate bus
       ret.mass = 1492 + STD_CARGO_KG
       ret.wheelbase = 2.824
       ret.centerToFront = ret.wheelbase * 0.44
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index 8ff9dad38..e32403776 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -21,7 +21,7 @@ class CarState(CarStateBase):
     if self.car_fingerprint in PREGLOBAL_CARS:
       ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 2
     else:
-      ret.brakePressed = cp.vl["Brake_Pedal"]["Brake_Pedal"] > 1e-5
+      ret.brakePressed = cp.vl["Brake_Status"]["Brake"] == 1
 
     ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]["FL"] * CV.KPH_TO_MS
     ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]["FR"] * CV.KPH_TO_MS
@@ -125,12 +125,14 @@ class CarState(CarStateBase):
     if CP.carFingerprint not in PREGLOBAL_CARS:
       signals += [
         ("Steer_Warning", "Steering_Torque", 0),
+        ("Brake", "Brake_Status", 0),
         ("UNITS", "Dashlights", 0),
       ]
 
       checks += [
         ("Dashlights", 10),
         ("BodyInfo", 10),
+        ("Brake_Status", 50),
         ("CruiseControl", 20),
       ]
     else:
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
index 260933cb2..86bba542c 100644
--- a/selfdrive/car/subaru/interface.py
+++ b/selfdrive/car/subaru/interface.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.car.subaru.values import CAR, PREGLOBAL_CARS
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 class CarInterface(CarInterfaceBase):
@@ -14,14 +14,12 @@ class CarInterface(CarInterfaceBase):
     ret.radarOffCan = True
 
     if candidate in PREGLOBAL_CARS:
-      ret.safetyModel = car.CarParams.SafetyModel.subaruLegacy
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaruLegacy)]
       ret.enableBsm = 0x25c in fingerprint[0]
     else:
-      ret.safetyModel = car.CarParams.SafetyModel.subaru
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.subaru)]
       ret.enableBsm = 0x228 in fingerprint[0]
 
-    # Subaru port is a community feature, since we don't own one to test
-    ret.communityFeature = True
     ret.dashcamOnly = candidate in PREGLOBAL_CARS
 
     ret.steerRateCost = 0.7
@@ -58,7 +56,7 @@ class CarInterface(CarInterfaceBase):
       ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01, 0.065, 0.2], [0.001, 0.015, 0.025]]
 
     if candidate in [CAR.FORESTER_PREGLOBAL, CAR.OUTBACK_PREGLOBAL_2018]:
-      ret.safetyParam = 1  # Outback 2018-2019 and Forester have reversed driver torque signal
+      ret.safetyConfigs[0].safetyParam = 1  # Outback 2018-2019 and Forester have reversed driver torque signal
       ret.mass = 1568 + STD_CARGO_KG
       ret.wheelbase = 2.67
       ret.centerToFront = ret.wheelbase * 0.5
diff --git a/selfdrive/car/tesla/interface.py b/selfdrive/car/tesla/interface.py
index 2078fa38b..e26421545 100755
--- a/selfdrive/car/tesla/interface.py
+++ b/selfdrive/car/tesla/interface.py
@@ -1,7 +1,7 @@
 #!/usr/bin/env python3
 from cereal import car
 from selfdrive.car.tesla.values import CAR
-from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness
+from selfdrive.car import STD_CARGO_KG, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 
@@ -10,7 +10,7 @@ class CarInterface(CarInterfaceBase):
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "tesla"
-    ret.safetyModel = car.CarParams.SafetyModel.tesla
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.tesla)]
 
     # There is no safe way to do steer blending with user torque,
     # so the steering behaves like autopilot. This is not
@@ -19,7 +19,6 @@ class CarInterface(CarInterfaceBase):
 
     ret.steerControlType = car.CarParams.SteerControlType.angle
     ret.openpilotLongitudinalControl = False
-    ret.communityFeature = True
 
     ret.steerActuatorDelay = 0.1
     ret.steerRateCost = 0.5
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 7a60ebc52..2d17ee20f 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -75,8 +75,9 @@ class CarController():
     else:
       apply_steer_req = 1
 
+    # TODO: probably can delete this. CS.pcm_acc_status uses a different signal
+    # than CS.cruiseState.enabled. confirm they're not meaningfully different
     if not enabled and CS.pcm_acc_status:
-      # send pcm acc cancel cmd if drive is disabled but pcm is still on, or if the system can't be activated
       pcm_cancel_cmd = 1
 
     # on entering standstill, send standstill request
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index a7790bdf9..83a152ac1 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -90,7 +90,7 @@ class CarState(CarStateBase):
 
     # some TSS2 cars have low speed lockout permanently set, so ignore on those cars
     # these cars are identified by an ACC_TYPE value of 2.
-    # TODO: it may be possible to avoid the lockout and gain stop and go if you
+    # TODO: it is possible to avoid the lockout and gain stop and go if you
     # send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
     if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
        (self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index e0a07cf2d..7453bda0d 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -2,7 +2,7 @@
 from cereal import car
 from selfdrive.config import Conversions as CV
 from selfdrive.car.toyota.values import Ecu, CAR, TSS2_CAR, NO_DSU_CAR, MIN_ACC_SPEED, PEDAL_HYST_GAP, CarControllerParams
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 EventName = car.CarEvent.EventName
@@ -18,7 +18,7 @@ class CarInterface(CarInterfaceBase):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
 
     ret.carName = "toyota"
-    ret.safetyModel = car.CarParams.SafetyModel.toyota
+    ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.toyota)]
 
     ret.steerActuatorDelay = 0.12  # Default delay, Prius has larger delay
     ret.steerLimitTimer = 0.4
@@ -31,7 +31,7 @@ class CarInterface(CarInterfaceBase):
 
     if candidate == CAR.PRIUS:
       stop_and_go = True
-      ret.safetyParam = 66  # see conversion factor for STEER_TORQUE_EPS in dbc file
+      ret.safetyConfigs[0].safetyParam = 66  # see conversion factor for STEER_TORQUE_EPS in dbc file
       ret.wheelbase = 2.70
       ret.steerRatio = 15.74   # unknown end-to-end spec
       tire_stiffness_factor = 0.6371   # hand-tune
@@ -50,7 +50,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.RAV4, CAR.RAV4H]:
       stop_and_go = True if (candidate in CAR.RAV4H) else False
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.65
       ret.steerRatio = 16.88   # 14.5 is spec end-to-end
       tire_stiffness_factor = 0.5533
@@ -69,7 +69,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.COROLLA:
       stop_and_go = False
-      ret.safetyParam = 88
+      ret.safetyConfigs[0].safetyParam = 88
       ret.wheelbase = 2.70
       ret.steerRatio = 18.27
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -79,7 +79,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_RX:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.79
       ret.steerRatio = 14.8
       tire_stiffness_factor = 0.5533
@@ -89,7 +89,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_RXH:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.79
       ret.steerRatio = 16.  # 14.8 is spec end-to-end
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -99,7 +99,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_RX_TSS2:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.79
       ret.steerRatio = 14.8
       tire_stiffness_factor = 0.5533  # not optimized yet
@@ -109,7 +109,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_RXH_TSS2:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.79
       ret.steerRatio = 16.0  # 14.8 is spec end-to-end
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -119,7 +119,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.CHR, CAR.CHRH]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.63906
       ret.steerRatio = 13.6
       tire_stiffness_factor = 0.7933
@@ -129,7 +129,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.CAMRY, CAR.CAMRYH, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.82448
       ret.steerRatio = 13.7
       tire_stiffness_factor = 0.7933
@@ -139,7 +139,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.84988  # 112.2 in = 2.84988 m
       ret.steerRatio = 16.0
       tire_stiffness_factor = 0.8
@@ -149,7 +149,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.HIGHLANDER, CAR.HIGHLANDERH]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.78
       ret.steerRatio = 16.0
       tire_stiffness_factor = 0.8
@@ -159,7 +159,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019]:
       stop_and_go = False
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.82
       ret.steerRatio = 14.8  # Found at https://pressroom.toyota.com/releases/2016+avalon+product+specs.download
       tire_stiffness_factor = 0.7983
@@ -169,7 +169,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.RAV4_TSS2, CAR.RAV4H_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.68986
       ret.steerRatio = 14.3
       tire_stiffness_factor = 0.7933
@@ -187,7 +187,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.67  # Average between 2.70 for sedan and 2.64 for hatchback
       ret.steerRatio = 13.9
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -197,7 +197,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.8702
       ret.steerRatio = 16.0  # not optimized
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -207,7 +207,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_ESH:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.8190
       ret.steerRatio = 16.06
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -217,7 +217,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.SIENNA:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 3.03
       ret.steerRatio = 15.5
       tire_stiffness_factor = 0.444
@@ -227,7 +227,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_IS:
       stop_and_go = False
-      ret.safetyParam = 77
+      ret.safetyConfigs[0].safetyParam = 77
       ret.wheelbase = 2.79908
       ret.steerRatio = 13.3
       tire_stiffness_factor = 0.444
@@ -237,7 +237,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.LEXUS_CTH:
       stop_and_go = True
-      ret.safetyParam = 100
+      ret.safetyConfigs[0].safetyParam = 100
       ret.wheelbase = 2.60
       ret.steerRatio = 18.6
       tire_stiffness_factor = 0.517
@@ -247,7 +247,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate in [CAR.LEXUS_NXH, CAR.LEXUS_NX, CAR.LEXUS_NX_TSS2]:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.66
       ret.steerRatio = 14.7
       tire_stiffness_factor = 0.444  # not optimized yet
@@ -257,7 +257,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.PRIUS_TSS2:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.70002  # from toyota online sepc.
       ret.steerRatio = 13.4   # True steerRation from older prius
       tire_stiffness_factor = 0.6371   # hand-tune
@@ -267,7 +267,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.MIRAI:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 2.91
       ret.steerRatio = 14.8
       tire_stiffness_factor = 0.8
@@ -277,7 +277,7 @@ class CarInterface(CarInterfaceBase):
 
     elif candidate == CAR.ALPHARD_TSS2:
       stop_and_go = True
-      ret.safetyParam = 73
+      ret.safetyConfigs[0].safetyParam = 73
       ret.wheelbase = 3.00
       ret.steerRatio = 14.2
       tire_stiffness_factor = 0.444
@@ -330,7 +330,6 @@ class CarInterface(CarInterfaceBase):
       ret.longitudinalTuning.kiV = [.35, .23, .20, .17, .1]
       ret.stoppingDecelRate = 0.3  # reach stopping target smoothly
       ret.startingAccelRate = 6.0  # release brakes fast
-      ret.startAccel = 1.2  # Accelerate from 0 faster
     else:
       # Default longitudinal tune
       ret.longitudinalTuning.deadzoneBP = [0., 9.]
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index 1bb41f102..43b06d9dc 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -57,10 +57,12 @@ def create_acc_cancel_command(packer):
 
 def create_fcw_command(packer, fcw):
   values = {
+    "PCS_INDICATOR": 1,
     "FCW": fcw,
     "SET_ME_X20": 0x20,
     "SET_ME_X10": 0x10,
-    "SET_ME_X80": 0x80,
+    "PCS_OFF": 1,
+    "PCS_SENSITIVITY": 0,
   }
   return packer.make_can_msg("ACC_HUD", 0, values)
 
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 3acaaf62e..b9312eef4 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -21,44 +21,47 @@ class CarControllerParams:
   STEER_ERROR_MAX = 350     # max delta between torque cmd and torque motor
 
 class CAR:
-  PRIUS = "TOYOTA PRIUS 2017"
-  PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
-  RAV4H = "TOYOTA RAV4 HYBRID 2017"
-  RAV4 = "TOYOTA RAV4 2017"
-  COROLLA = "TOYOTA COROLLA 2017"
-  LEXUS_RX = "LEXUS RX 2016"
-  LEXUS_RXH = "LEXUS RX HYBRID 2017"
-  LEXUS_RX_TSS2 = "LEXUS RX 2020"
-  LEXUS_RXH_TSS2 = "LEXUS RX HYBRID 2020"
-  CHR = "TOYOTA C-HR 2018"
-  CHRH = "TOYOTA C-HR HYBRID 2018"
+  # Toyota
+  ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
+  AVALON = "TOYOTA AVALON 2016"
+  AVALON_2019 = "TOYOTA AVALON 2019"
+  AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
   CAMRY = "TOYOTA CAMRY 2018"
   CAMRYH = "TOYOTA CAMRY HYBRID 2018"
   CAMRY_TSS2 = "TOYOTA CAMRY 2021"  # TSS 2.5
   CAMRYH_TSS2 = "TOYOTA CAMRY HYBRID 2021"
+  CHR = "TOYOTA C-HR 2018"
+  CHRH = "TOYOTA C-HR HYBRID 2018"
+  COROLLA = "TOYOTA COROLLA 2017"
+  COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
+  # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
+  COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019"
   HIGHLANDER = "TOYOTA HIGHLANDER 2017"
   HIGHLANDER_TSS2 = "TOYOTA HIGHLANDER 2020"
   HIGHLANDERH = "TOYOTA HIGHLANDER HYBRID 2018"
   HIGHLANDERH_TSS2 = "TOYOTA HIGHLANDER HYBRID 2020"
-  AVALON = "TOYOTA AVALON 2016"
-  AVALON_2019 = "TOYOTA AVALON 2019"
-  AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
+  PRIUS = "TOYOTA PRIUS 2017"
+  PRIUS_TSS2 = "TOYOTA PRIUS TSS2 2021"
+  RAV4 = "TOYOTA RAV4 2017"
+  RAV4H = "TOYOTA RAV4 HYBRID 2017"
   RAV4_TSS2 = "TOYOTA RAV4 2019"
-  COROLLA_TSS2 = "TOYOTA COROLLA TSS2 2019"
-  # LSS2 Lexus UX Hybrid is same as a TSS2 Corolla Hybrid
-  COROLLAH_TSS2 = "TOYOTA COROLLA HYBRID TSS2 2019"
+  RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
+  MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
+  SIENNA = "TOYOTA SIENNA 2018"
+
+  # Lexus
+  LEXUS_CTH = "LEXUS CT HYBRID 2018"
+  LEXUS_ESH = "LEXUS ES HYBRID 2018"
   LEXUS_ES_TSS2 = "LEXUS ES 2019"
   LEXUS_ESH_TSS2 = "LEXUS ES HYBRID 2019"
-  LEXUS_ESH = "LEXUS ES HYBRID 2018"
-  SIENNA = "TOYOTA SIENNA 2018"
   LEXUS_IS = "LEXUS IS 2018"
-  LEXUS_CTH = "LEXUS CT HYBRID 2018"
-  RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
-  LEXUS_NXH = "LEXUS NX HYBRID 2018"
   LEXUS_NX = "LEXUS NX 2018"
+  LEXUS_NXH = "LEXUS NX HYBRID 2018"
   LEXUS_NX_TSS2 = "LEXUS NX 2020"
-  MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
-  ALPHARD_TSS2 = "TOYOTA ALPHARD 2020"
+  LEXUS_RX = "LEXUS RX 2016"
+  LEXUS_RXH = "LEXUS RX HYBRID 2017"
+  LEXUS_RX_TSS2 = "LEXUS RX 2020"
+  LEXUS_RXH_TSS2 = "LEXUS RX HYBRID 2020"
 
 # (addr, cars, bus, 1/freq*100, vl)
 STATIC_DSU_MSGS = [
@@ -84,16 +87,6 @@ STATIC_DSU_MSGS = [
 
 
 FINGERPRINTS = {
-  CAR.RAV4: [{
-    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2015: 8, 2024: 8
-  }],
-  CAR.RAV4H: [{
-    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 296: 8, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 550: 8, 552: 4, 560: 7, 562: 4, 581: 5, 608: 8, 610: 5, 643: 7, 705: 8, 713: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1212: 8, 1227: 8, 1228: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1656: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  # Chinese RAV4
-  {
-    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
-  }],
   CAR.PRIUS: [{
     36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
   },
@@ -117,77 +110,6 @@ FINGERPRINTS = {
   {
     36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 767: 4, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1792: 8, 1767: 4, 1863: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1990: 8, 1992: 8, 1996: 8, 1998: 8, 2002: 8, 2010: 8, 2015: 8, 2016: 8, 2018: 8, 2024: 8, 2026: 8, 2030: 8
   }],
-  #Corolla w/ added Pedal Support (512L and 513L)
-  CAR.COROLLA: [{
-    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
-  }],
-  CAR.CAMRY: [
-  #XLE and LE
-  {
-    36: 8, 37: 8, 119: 6, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  #XSE and SE
-  # TODO: get proper fingerprint in stock mode
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 888: 8, 889: 8, 891: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1816: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  {
-  # 2019 XSE
-    36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 550: 8, 552: 4, 562: 6, 608: 8, 610: 8, 643: 7, 658: 8, 705: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 822: 8, 824: 8, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 891: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 942: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 976: 1, 983: 8, 984: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1412: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1792: 8, 1767: 4, 1808: 8, 1816: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1937: 8, 1945: 8, 1953: 8, 1961: 8, 1968: 8, 1976: 8, 1990: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
-  }],
-  CAR.CAMRYH: [
-  #SE, LE and LE with Blindspot Monitor
-  {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 889: 8, 896: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1649: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  #SL
-  {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  #XLE
-  {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 658: 8, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 767: 4, 800: 8, 810: 2, 812: 8, 814: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  }],
-  CAR.HIGHLANDER: [{
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1984: 8, 1988: 8, 1992: 8, 1996: 8, 1990: 8, 1998: 8
-  },
-  # 2019 Highlander XLE
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  # 2017 Highlander Limited
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  # 2018 Highlander Limited Platinum
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1585: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1872: 8, 1880: 8, 1904: 8, 1912: 8, 1988: 8, 1990: 8, 1996: 8, 1998: 8, 2015: 8, 2016: 8, 2024: 8
-  }],
-  CAR.HIGHLANDERH: [{
-    36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  {
-  # 2019 Highlander Hybrid Limited Platinum
-    36: 8, 37: 8, 170: 8, 180: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 581: 5, 608: 8, 610: 5, 643: 7, 713: 8, 740: 5, 767: 4, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 918: 7, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 3, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1076: 8, 1077: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1184: 8, 1185: 8, 1186: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1212: 8, 1227: 8, 1232: 8, 1235: 8, 1237: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  }],
-  CAR.COROLLAH_TSS2: [
-  # 2019 Taiwan Altis Hybrid
-  {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 767: 4, 800: 8, 810: 2, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 918: 7, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 987: 8, 993: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1082: 8, 1112: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1237: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1745: 8, 1775: 8, 1779: 8
-  },
-  # 2019 Chinese Levin Hybrid
-  {
-    36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 401: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 742: 8, 743: 8, 761: 8, 765: 8, 767: 4, 800: 8, 810: 2, 812: 8, 829: 2, 830: 7, 835: 8, 836: 8, 865: 8, 869: 7, 870: 7, 871: 2, 877: 8, 881: 8, 885: 8, 896: 8, 898: 8, 921: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 1002: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1172: 8, 1235: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1594: 8, 1595: 8, 1600: 8, 1649: 8, 1745: 8, 1775: 8, 1779: 8
-  }
-  ],
-  CAR.SIENNA: [
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767: 4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 888: 8, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 918: 7, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  },
-  # XLE AWD 2018
-  {
-    36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 548: 8, 550: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 764: 8, 767: 4, 800: 8, 824: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 1, 921: 8, 933: 8, 944: 6, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1008: 2, 1014: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1056: 8, 1059: 1, 1076: 8, 1077: 8, 1114: 8, 1160: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1200: 8, 1201: 8, 1202: 8, 1203: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1656: 8, 1664: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
-  }],
 }
 
 
@@ -289,6 +211,7 @@ FW_VERSIONS = {
       b'\x018966333P4400\x00\x00\x00\x00',
       b'\x018966333P4500\x00\x00\x00\x00',
       b'\x018966333P4700\x00\x00\x00\x00',
+      b'\x018966333P4900\x00\x00\x00\x00',
       b'\x018966333Q6000\x00\x00\x00\x00',
       b'\x018966333Q6200\x00\x00\x00\x00',
       b'\x018966333Q6300\x00\x00\x00\x00',
@@ -360,6 +283,7 @@ FW_VERSIONS = {
       b'\x01896633T16000\x00\x00\x00\x00',
       b'\x028966306B2100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x028966306B2300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966306B2500\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x028966306N8100\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x028966306N8200\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
       b'\x028966306N8300\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
@@ -385,6 +309,7 @@ FW_VERSIONS = {
       b'8821F0601300    ',
       b'8821F0603400    ',
       b'8821F0604000    ',
+      b'8821F0604100    ',
       b'8821F0604200    ',
       b'8821F0605200    ',
       b'8821F0606200    ',
@@ -409,6 +334,7 @@ FW_VERSIONS = {
       b'8821F0601300    ',
       b'8821F0603400    ',
       b'8821F0604000    ',
+      b'8821F0604100    ',
       b'8821F0604200    ',
       b'8821F0605200    ',
       b'8821F0606200    ',
@@ -511,6 +437,7 @@ FW_VERSIONS = {
       b'\x0331024000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
       b'\x0331036000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
       b'\x033F401100\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203102\x00\x00\x00\x00',
+      b'\x033F401200\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
       b'\x033F424000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
@@ -552,6 +479,7 @@ FW_VERSIONS = {
       b'8821FF402300    ',
       b'8821FF402400    ',
       b'8821FF404000    ',
+      b'8821FF404100    ',
       b'8821FF406000    ',
       b'8821FF407100    ',
     ],
@@ -566,6 +494,7 @@ FW_VERSIONS = {
       b'8821FF402300    ',
       b'8821FF402400    ',
       b'8821FF404000    ',
+      b'8821FF404100    ',
       b'8821FF406000    ',
       b'8821FF407100    ',
     ],
@@ -625,6 +554,7 @@ FW_VERSIONS = {
       b'\x018966312P9000\x00\x00\x00\x00',
       b'\x018966312P9100\x00\x00\x00\x00',
       b'\x018966312P9200\x00\x00\x00\x00',
+      b'\x018966312P9300\x00\x00\x00\x00',
       b'\x018966312Q2300\x00\x00\x00\x00',
       b'\x018966312R0000\x00\x00\x00\x00',
       b'\x018966312R0100\x00\x00\x00\x00',
@@ -634,9 +564,12 @@ FW_VERSIONS = {
       b'\x018966312S5000\x00\x00\x00\x00',
       b'\x018966312S7000\x00\x00\x00\x00',
       b'\x018966312W3000\x00\x00\x00\x00',
+      b'\x018966312W9000\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x7e0, None): [
+      b'\x0230A11000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x0230ZN4000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00',
+      b'\x03312K7000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
       b'\x03312M3000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203402\x00\x00\x00\x00',
       b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203202\x00\x00\x00\x00',
       b'\x03312N6000\x00\x00\x00\x00\x00\x00\x00\x00A0202000\x00\x00\x00\x00\x00\x00\x00\x00895231203302\x00\x00\x00\x00',
@@ -653,18 +586,21 @@ FW_VERSIONS = {
       b'\x018965B12530\x00\x00\x00\x00\x00\x00',
       b'\x018965B1255000\x00\x00\x00\x00',
       b'8965B12361\x00\x00\x00\x00\x00\x00',
+      b'8965B16011\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.esp, 0x7b0, None): [
       b'\x01F152602280\x00\x00\x00\x00\x00\x00',
       b'\x01F152602560\x00\x00\x00\x00\x00\x00',
       b'\x01F152602590\x00\x00\x00\x00\x00\x00',
       b'\x01F152602650\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260A050\x00\x00\x00\x00\x00\x00',
       b'\x01F152612641\x00\x00\x00\x00\x00\x00',
       b'\x01F152612651\x00\x00\x00\x00\x00\x00',
       b'\x01F152612B10\x00\x00\x00\x00\x00\x00',
       b'\x01F152612B51\x00\x00\x00\x00\x00\x00',
       b'\x01F152612B60\x00\x00\x00\x00\x00\x00',
       b'\x01F152612B61\x00\x00\x00\x00\x00\x00',
+      b'\x01F152612B71\x00\x00\x00\x00\x00\x00',
       b'\x01F152612B90\x00\x00\x00\x00\x00\x00',
       b'\x01F152612C00\x00\x00\x00\x00\x00\x00',
       b'F152602191\x00\x00\x00\x00\x00\x00',
@@ -684,6 +620,7 @@ FW_VERSIONS = {
       b'\x028646F1202000\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
       b'\x028646F1202100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
       b'\x028646F1202200\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
+      b'\x028646F1601100\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
   },
   CAR.COROLLAH_TSS2: {
@@ -700,6 +637,8 @@ FW_VERSIONS = {
       b'\x02896630ZR2000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896630ZT8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896630ZT9000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312K6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x028966312L0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x028966312Q3000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x028966312Q4000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x038966312L7000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF1205001\x00\x00\x00\x00',
@@ -714,6 +653,7 @@ FW_VERSIONS = {
       b'\x018965B12470\x00\x00\x00\x00\x00\x00',
       b'\x018965B12490\x00\x00\x00\x00\x00\x00',
       b'\x018965B12500\x00\x00\x00\x00\x00\x00',
+      b'\x018965B12510\x00\x00\x00\x00\x00\x00',
       b'\x018965B12520\x00\x00\x00\x00\x00\x00',
       b'\x018965B12530\x00\x00\x00\x00\x00\x00',
     ],
@@ -727,6 +667,7 @@ FW_VERSIONS = {
       b'F152612800\x00\x00\x00\x00\x00\x00',
       b'F152612820\x00\x00\x00\x00\x00\x00',
       b'F152612840\x00\x00\x00\x00\x00\x00',
+      b'F152612890\x00\x00\x00\x00\x00\x00',
       b'F152612A00\x00\x00\x00\x00\x00\x00',
       b'F152612A10\x00\x00\x00\x00\x00\x00',
       b'F152642540\x00\x00\x00\x00\x00\x00',
@@ -822,9 +763,11 @@ FW_VERSIONS = {
     (Ecu.eps, 0x7a1, None): [
       b'8965B48241\x00\x00\x00\x00\x00\x00',
       b'8965B48310\x00\x00\x00\x00\x00\x00',
+      b'8965B48320\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.esp, 0x7b0, None): [
       b'\x01F15260E051\x00\x00\x00\x00\x00\x00',
+      b'\x01F15260E061\x00\x00\x00\x00\x00\x00',
       b'\x01F15260E110\x00\x00\x00\x00\x00\x00',
     ],
     (Ecu.engine, 0x700, None): [
@@ -838,6 +781,7 @@ FW_VERSIONS = {
       b'\x01896630EB2000\x00\x00\x00\x00',
       b'\x01896630EB2100\x00\x00\x00\x00',
       b'\x01896630EB2200\x00\x00\x00\x00',
+      b'\x01896630EC4000\x00\x00\x00\x00',
     ],
     (Ecu.fwdRadar, 0x750, 0xf): [
       b'\x018821F3301400\x00\x00\x00\x00',
@@ -928,6 +872,7 @@ FW_VERSIONS = {
       b'\x02896634763100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896634765000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896634765100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
+      b'\x02896634769000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896634769100\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896634769200\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x02896634770000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
@@ -981,6 +926,7 @@ FW_VERSIONS = {
       b'F152647417\x00\x00\x00\x00\x00\x00',
       b'F152647470\x00\x00\x00\x00\x00\x00',
       b'F152647490\x00\x00\x00\x00\x00\x00',
+      b'F152647682\x00\x00\x00\x00\x00\x00',
       b'F152647683\x00\x00\x00\x00\x00\x00',
       b'F152647684\x00\x00\x00\x00\x00\x00',
       b'F152647862\x00\x00\x00\x00\x00\x00',
@@ -1098,6 +1044,7 @@ FW_VERSIONS = {
       b'\x018966342T9000\x00\x00\x00\x00',
       b'\x018966342U4000\x00\x00\x00\x00',
       b'\x018966342U4100\x00\x00\x00\x00',
+      b'\x018966342U5100\x00\x00\x00\x00',
       b'\x018966342V3000\x00\x00\x00\x00',
       b'\x018966342V3100\x00\x00\x00\x00',
       b'\x018966342V3200\x00\x00\x00\x00',
@@ -1169,9 +1116,11 @@ FW_VERSIONS = {
       b'\x01896634A25000\x00\x00\x00\x00',
       b'\x018966342W5000\x00\x00\x00\x00',
       b'\x028966342W4001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
+      b'\x02896634A13000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02896634A13001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A13101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A14001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
+      b'\x02896634A23000\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00',
       b'\x02896634A23001\x00\x00\x00\x00897CF1203001\x00\x00\x00\x00',
       b'\x02896634A14001\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
       b'\x02896634A14101\x00\x00\x00\x00897CF4801001\x00\x00\x00\x00',
@@ -1179,6 +1128,7 @@ FW_VERSIONS = {
     (Ecu.esp, 0x7b0, None): [
       b'F152642291\x00\x00\x00\x00\x00\x00',
       b'F152642290\x00\x00\x00\x00\x00\x00',
+      b'F152642322\x00\x00\x00\x00\x00\x00',
       b'F152642330\x00\x00\x00\x00\x00\x00',
       b'F152642331\x00\x00\x00\x00\x00\x00',
       b'F152642531\x00\x00\x00\x00\x00\x00',
@@ -1296,6 +1246,7 @@ FW_VERSIONS = {
   CAR.LEXUS_ESH_TSS2: {
     (Ecu.engine, 0x700, None): [
       b'\x028966333S8000\x00\x00\x00\x00897CF3302002\x00\x00\x00\x00',
+      b'\x028966333S8000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x028966333T0100\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x028966333V4000\x00\x00\x00\x00897CF3305001\x00\x00\x00\x00',
       b'\x02896633T09000\x00\x00\x00\x00897CF3307001\x00\x00\x00\x00',
@@ -1319,6 +1270,7 @@ FW_VERSIONS = {
     (Ecu.fwdCamera, 0x750, 0x6d): [
       b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F3303100\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
+      b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
       b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
       b'\x028646F3304200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
     ],
@@ -1568,6 +1520,7 @@ FW_VERSIONS = {
   },
   CAR.PRIUS_TSS2: {
     (Ecu.engine, 0x700, None): [
+      b'\x028966347C6000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x028966347C8000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00',
       b'\x038966347C0000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
       b'\x038966347C1000\x00\x00\x00\x008966A4703000\x00\x00\x00\x00897CF4710101\x00\x00\x00\x00',
diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py
index 970ba8782..97f00008b 100644
--- a/selfdrive/car/volkswagen/carcontroller.py
+++ b/selfdrive/car/volkswagen/carcontroller.py
@@ -39,7 +39,7 @@ class CarController():
       # torque value. Do that anytime we happen to have 0 torque, or failing that,
       # when exceeding ~1/3 the 360 second timer.
 
-      if enabled and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
+      if enabled and CS.out.vEgo > CS.CP.minSteerSpeed and not (CS.out.standstill or CS.out.steerError or CS.out.steerWarning):
         new_steer = int(round(actuators.steer * P.STEER_MAX))
         apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, P)
         self.steer_rate_limited = new_steer != apply_steer
@@ -79,9 +79,7 @@ class CarController():
 
       can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, CANBUS.pt, enabled,
                                                             CS.out.steeringPressed, hud_alert, left_lane_visible,
-                                                            right_lane_visible, CS.ldw_lane_warning_left,
-                                                            CS.ldw_lane_warning_right, CS.ldw_side_dlc_tlc,
-                                                            CS.ldw_dlc, CS.ldw_tlc, CS.out.standstill,
+                                                            right_lane_visible, CS.ldw_stock_values,
                                                             left_lane_depart, right_lane_depart))
 
     # **** ACC Button Controls ********************************************** #
@@ -96,7 +94,6 @@ class CarController():
       elif enabled and CS.out.standstill:
         # Blip the Resume button if we're engaged at standstill.
         # FIXME: This is a naive implementation, improve with visiond or radar input.
-        # A subset of MQBs like to "creep" too aggressively with this implementation.
         self.graButtonStatesToSend = BUTTON_STATES.copy()
         self.graButtonStatesToSend["resumeCruise"] = True
 
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index f74484bd2..f02129a61 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -27,8 +27,7 @@ class CarState(CarStateBase):
 
     ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
     ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
-
-    ret.standstill = ret.vEgoRaw < 0.1
+    ret.standstill = bool(pt_cp.vl["ESP_21"]["ESP_Haltebestaetigung"])
 
     # Update steering angle, rate, yaw rate, and driver input torque. VW send
     # the sign/direction in a separate signal so they must be recombined.
@@ -84,11 +83,7 @@ class CarState(CarStateBase):
 
     # Consume factory LDW data relevant for factory SWA (Lane Change Assist)
     # and capture it for forwarding to the blind spot radar controller
-    self.ldw_lane_warning_left = bool(cam_cp.vl["LDW_02"]["LDW_SW_Warnung_links"])
-    self.ldw_lane_warning_right = bool(cam_cp.vl["LDW_02"]["LDW_SW_Warnung_rechts"])
-    self.ldw_side_dlc_tlc = bool(cam_cp.vl["LDW_02"]["LDW_Seite_DLCTLC"])
-    self.ldw_dlc = cam_cp.vl["LDW_02"]["LDW_DLC"]
-    self.ldw_tlc = cam_cp.vl["LDW_02"]["LDW_TLC"]
+    self.ldw_stock_values = cam_cp.vl["LDW_02"] if self.CP.networkLocation == NetworkLocation.fwdCamera else {}
 
     # Stock FCW is considered active if the release bit for brake-jerk warning
     # is set. Stock AEB considered active if the partial braking or target
@@ -177,6 +172,7 @@ class CarState(CarStateBase):
       ("EPS_VZ_Lenkmoment", "LH_EPS_03", 0),        # Driver torque input sign
       ("EPS_HCA_Status", "LH_EPS_03", 3),           # EPS HCA control status
       ("ESP_Tastung_passiv", "ESP_21", 0),          # Stability control disabled
+      ("ESP_Haltebestaetigung", "ESP_21", 0),       # ESP hold confirmation
       ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0),  # MPH vs KMH speed display
       ("KBI_Handbremse", "Kombi_01", 0),            # Manual handbrake applied
       ("TSK_Status", "TSK_06", 0),                  # ACC engagement status from drivetrain coordinator
@@ -235,21 +231,23 @@ class CarState(CarStateBase):
   @staticmethod
   def get_cam_can_parser(CP):
 
-    signals = [
-      # sig_name, sig_address, default
-      ("LDW_SW_Warnung_links", "LDW_02", 0),          # Blind spot in warning mode on left side due to lane departure
-      ("LDW_SW_Warnung_rechts", "LDW_02", 0),         # Blind spot in warning mode on right side due to lane departure
-      ("LDW_Seite_DLCTLC", "LDW_02", 0),              # Direction of most likely lane departure (left or right)
-      ("LDW_DLC", "LDW_02", 0),                       # Lane departure, distance to line crossing
-      ("LDW_TLC", "LDW_02", 0),                       # Lane departure, time to line crossing
-    ]
+    signals = []
+    checks = []
 
-    checks = [
-      # sig_address, frequency
-      ("LDW_02", 10)        # From R242 Driver assistance camera
-    ]
-
-    if CP.networkLocation == NetworkLocation.gateway:
+    if CP.networkLocation == NetworkLocation.fwdCamera:
+      signals += [
+        # sig_name, sig_address, default
+        ("LDW_SW_Warnung_links", "LDW_02", 0),      # Blind spot in warning mode on left side due to lane departure
+        ("LDW_SW_Warnung_rechts", "LDW_02", 0),     # Blind spot in warning mode on right side due to lane departure
+        ("LDW_Seite_DLCTLC", "LDW_02", 0),          # Direction of most likely lane departure (left or right)
+        ("LDW_DLC", "LDW_02", 0),                   # Lane departure, distance to line crossing
+        ("LDW_TLC", "LDW_02", 0),                   # Lane departure, time to line crossing
+      ]
+      checks += [
+        # sig_address, frequency
+        ("LDW_02", 10)      # From R242 Driver assistance camera
+      ]
+    else:
       # Radars are here on CANBUS.cam
       signals += MqbExtraSignals.fwd_radar_signals
       checks += MqbExtraSignals.fwd_radar_checks
diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py
index 0f3fa8a4a..51d5af8bc 100644
--- a/selfdrive/car/volkswagen/interface.py
+++ b/selfdrive/car/volkswagen/interface.py
@@ -1,6 +1,6 @@
 from cereal import car
 from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES, CANBUS, NetworkLocation, TransmissionType, GearShifter
-from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint
+from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config
 from selfdrive.car.interfaces import CarInterfaceBase
 
 EventName = car.CarEvent.EventName
@@ -24,24 +24,23 @@ class CarInterface(CarInterfaceBase):
   def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None):
     ret = CarInterfaceBase.get_std_params(candidate, fingerprint)
     ret.carName = "volkswagen"
-    ret.communityFeature = True
     ret.radarOffCan = True
 
     if True:  # pylint: disable=using-constant-test
       # Set global MQB parameters
-      ret.safetyModel = car.CarParams.SafetyModel.volkswagen
-      ret.enableBsm = 0x30F in fingerprint[0]
+      ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.volkswagen)]
+      ret.enableBsm = 0x30F in fingerprint[0]  # SWA_01
 
       if 0xAD in fingerprint[0]:  # Getriebe_11
         ret.transmissionType = TransmissionType.automatic
       elif 0x187 in fingerprint[0]:  # EV_Gearshift
         ret.transmissionType = TransmissionType.direct
-      else:  # No trans message at all, must be a true stick-shift manual
+      else:
         ret.transmissionType = TransmissionType.manual
 
-      if 0x86 in fingerprint[1]:  # LWI_01 seen on bus 1, we're wired to the CAN gateway
+      if any(msg in fingerprint[1] for msg in [0x40, 0x86, 0xB2, 0xFD]):  # Airbag_01, LWI_01, ESP_19, ESP_21
         ret.networkLocation = NetworkLocation.gateway
-      else:  # We're wired to the LKAS camera
+      else:
         ret.networkLocation = NetworkLocation.fwdCamera
 
     # Global tuning defaults, can be overridden per-vehicle
@@ -59,7 +58,11 @@ class CarInterface(CarInterfaceBase):
 
     # Per-chassis tuning values, override tuning defaults here if desired
 
-    if candidate == CAR.ATLAS_MK1:
+    if candidate == CAR.ARTEON_MK1:
+      ret.mass = 1733 + STD_CARGO_KG
+      ret.wheelbase = 2.84
+
+    elif candidate == CAR.ATLAS_MK1:
       ret.mass = 2011 + STD_CARGO_KG
       ret.wheelbase = 2.98
 
@@ -75,6 +78,10 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1551 + STD_CARGO_KG
       ret.wheelbase = 2.79
 
+    elif candidate == CAR.TAOS_MK1:
+      ret.mass = 1498 + STD_CARGO_KG
+      ret.wheelbase = 2.69
+
     elif candidate == CAR.TCROSS_MK1:
       ret.mass = 1150 + STD_CARGO_KG
       ret.wheelbase = 2.60
@@ -87,6 +94,11 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1516 + STD_CARGO_KG
       ret.wheelbase = 2.79
 
+    elif candidate == CAR.TRANSPORTER_T61:
+      ret.mass = 1926 + STD_CARGO_KG
+      ret.wheelbase = 3.00  # SWB, LWB is 3.40, TBD how to detect difference
+      ret.minSteerSpeed = 14.0
+
     elif candidate == CAR.AUDI_A3_MK3:
       ret.mass = 1335 + STD_CARGO_KG
       ret.wheelbase = 2.61
@@ -103,6 +115,14 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1227 + STD_CARGO_KG
       ret.wheelbase = 2.64
 
+    elif candidate == CAR.SKODA_KAMIQ_MK1:
+      ret.mass = 1265 + STD_CARGO_KG
+      ret.wheelbase = 2.66
+
+    elif candidate == CAR.SKODA_KAROQ_MK1:
+      ret.mass = 1278 + STD_CARGO_KG
+      ret.wheelbase = 2.66
+
     elif candidate == CAR.SKODA_KODIAQ_MK1:
       ret.mass = 1569 + STD_CARGO_KG
       ret.wheelbase = 2.79
@@ -119,16 +139,13 @@ class CarInterface(CarInterfaceBase):
       ret.mass = 1505 + STD_CARGO_KG
       ret.wheelbase = 2.84
 
-    # TODO: get actual value, for now starting with reasonable value for
-    # civic and scaling by mass and wheelbase
-    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
+    else:
+      raise ValueError("unsupported car %s" % candidate)
 
-    # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
-    # mass and CG position, so all cars will have approximately similar dyn behaviors
+    ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase)
     ret.centerToFront = ret.wheelbase * 0.45
     ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront,
                                                                          tire_stiffness_factor=tire_stiffness_factor)
-
     return ret
 
   # returns a car.CarState
@@ -166,6 +183,14 @@ class CarInterface(CarInterfaceBase):
     if self.CS.parkingBrakeSet:
       events.add(EventName.parkBrake)
 
+    # Low speed steer alert hysteresis logic
+    if self.CP.minSteerSpeed > 0. and ret.vEgo < (self.CP.minSteerSpeed + 1.):
+      self.low_speed_alert = True
+    elif ret.vEgo > (self.CP.minSteerSpeed + 2.):
+      self.low_speed_alert = False
+    if self.low_speed_alert:
+      events.add(EventName.belowSteerSpeed)
+
     ret.events = events.to_msg()
     ret.buttonEvents = buttonEvents
 
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 794c9e9d3..162462f1c 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -65,21 +65,26 @@ MQB_LDW_MESSAGES = {
 # Exception: SEAT Leon and SEAT Ateca share a chassis code
 
 class CAR:
-  ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN"      # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport
-  GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN"        # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants
-  JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN"      # Chassis BU, Mk7 Jetta
-  PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN"    # Chassis 3G, Mk8 Passat and variants
-  TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN"   # Chassis C1, Mk1 VW T-Cross SWB and LWB variants
-  TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN"    # Chassis AD/BW, Mk2 VW Tiguan and variants
-  TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN"    # Chassis 1T, Mk2 VW Touran and variants
-  AUDI_A3_MK3 = "AUDI A3 3RD GEN"             # Chassis 8V/FF, Mk3 Audi A3 and variants
-  AUDI_Q2_MK1 = "AUDI Q2 1ST GEN"             # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only)
-  SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN"       # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
-  SEAT_LEON_MK3 = "SEAT LEON 3RD GEN"         # Chassis 5F, Mk3 SEAT Leon and variants
-  SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN"   # Chassis NS, Mk1 Skoda Kodiaq
-  SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN"     # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq
-  SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN"   # Chassis 3V/NP, Mk3 Skoda Superb and variants
-  SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN" # Chassis NE, Mk3 Skoda Octavia and variants
+  ARTEON_MK1 = "VOLKSWAGEN ARTEON 1ST GEN"          # Chassis AN, Mk1 VW Arteon and variants
+  ATLAS_MK1 = "VOLKSWAGEN ATLAS 1ST GEN"            # Chassis CA, Mk1 VW Atlas and Atlas Cross Sport
+  GOLF_MK7 = "VOLKSWAGEN GOLF 7TH GEN"              # Chassis 5G/AU/BA/BE, Mk7 VW Golf and variants
+  JETTA_MK7 = "VOLKSWAGEN JETTA 7TH GEN"            # Chassis BU, Mk7 Jetta
+  PASSAT_MK8 = "VOLKSWAGEN PASSAT 8TH GEN"          # Chassis 3G, Mk8 Passat and variants
+  TAOS_MK1 = "VOLKSWAGEN TAOS 1ST GEN"              # Chassis B2, Mk1 VW Taos and Tharu
+  TCROSS_MK1 = "VOLKSWAGEN T-CROSS 1ST GEN"         # Chassis C1, Mk1 VW T-Cross SWB and LWB variants
+  TIGUAN_MK2 = "VOLKSWAGEN TIGUAN 2ND GEN"          # Chassis AD/BW, Mk2 VW Tiguan and variants
+  TOURAN_MK2 = "VOLKSWAGEN TOURAN 2ND GEN"          # Chassis 1T, Mk2 VW Touran and variants
+  TRANSPORTER_T61 = "VOLKSWAGEN TRANSPORTER T6.1"   # Chassis 7H/7L, T6-facelift Transporter/Multivan/Caravelle/California
+  AUDI_A3_MK3 = "AUDI A3 3RD GEN"                   # Chassis 8V/FF, Mk3 Audi A3 and variants
+  AUDI_Q2_MK1 = "AUDI Q2 1ST GEN"                   # Chassis GA, Mk1 Audi Q2 (RoW) and Q2L (China only)
+  SEAT_ATECA_MK1 = "SEAT ATECA 1ST GEN"             # Chassis 5F, Mk1 SEAT Ateca and CUPRA Ateca
+  SEAT_LEON_MK3 = "SEAT LEON 3RD GEN"               # Chassis 5F, Mk3 SEAT Leon and variants
+  SKODA_KAMIQ_MK1 = "SKODA KAMIQ 1ST GEN"           # Chassis NW, Mk1 Skoda Kamiq
+  SKODA_KAROQ_MK1 = "SKODA KAROQ 1ST GEN"           # Chassis NU, Mk1 Skoda Karoq
+  SKODA_KODIAQ_MK1 = "SKODA KODIAQ 1ST GEN"         # Chassis NS, Mk1 Skoda Kodiaq
+  SKODA_SCALA_MK1 = "SKODA SCALA 1ST GEN"           # Chassis NW, Mk1 Skoda Scala and Skoda Kamiq
+  SKODA_SUPERB_MK3 = "SKODA SUPERB 3RD GEN"         # Chassis 3V/NP, Mk3 Skoda Superb and variants
+  SKODA_OCTAVIA_MK3 = "SKODA OCTAVIA 3RD GEN"       # Chassis NE, Mk3 Skoda Octavia and variants
 
 # All supported cars should return FW from the engine, srs, eps, and fwdRadar. Cars
 # with a manual trans won't return transmission firmware, but all other cars will.
@@ -91,6 +96,23 @@ class CAR:
 # them repaired by the tuner before including them in openpilot.
 
 FW_VERSIONS = {
+  CAR.ARTEON_MK1: {
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x873G0906259P \xf1\x890001',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x8709G927158L \xf1\x893611',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x873Q0959655DL\xf1\x890732\xf1\x82\0161812141812171105141123052J00',
+    ],
+    (Ecu.eps, 0x712, None): [
+      b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567B0020800',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x872Q0907572T \xf1\x890383',
+    ],
+  },
   CAR.ATLAS_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8703H906026AA\xf1\x899970',
@@ -128,6 +150,8 @@ FW_VERSIONS = {
       b'\xf1\x8704E906027GR\xf1\x892394',
       b'\xf1\x8704E906027HD\xf1\x893742',
       b'\xf1\x8704E906027MA\xf1\x894958',
+      b'\xf1\x8704L906021DT\xf1\x895520',
+      b'\xf1\x8704L906021N \xf1\x895518',
       b'\xf1\x8704L906026BP\xf1\x897608',
       b'\xf1\x8704L906026NF\xf1\x899528',
       b'\xf1\x8704L906056CL\xf1\x893823',
@@ -158,6 +182,7 @@ FW_VERSIONS = {
       b'\xf1\x870CW300041H \xf1\x891010',
       b'\xf1\x870CW300042F \xf1\x891604',
       b'\xf1\x870CW300043B \xf1\x891601',
+      b'\xf1\x870CW300044S \xf1\x894530',
       b'\xf1\x870CW300045  \xf1\x894531',
       b'\xf1\x870CW300047D \xf1\x895261',
       b'\xf1\x870CW300048J \xf1\x890611',
@@ -167,6 +192,7 @@ FW_VERSIONS = {
       b'\xf1\x870D9300012  \xf1\x895045',
       b'\xf1\x870D9300014M \xf1\x895004',
       b'\xf1\x870D9300020S \xf1\x895201',
+      b'\xf1\x870D9300040A \xf1\x893613',
       b'\xf1\x870D9300040S \xf1\x894311',
       b'\xf1\x870D9300041H \xf1\x895220',
       b'\xf1\x870DD300045K \xf1\x891120',
@@ -188,6 +214,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655BH\xf1\x890336\xf1\x82\02314160011123300314211012230229333463100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\023141600111233003142404A2252229333463100',
       b'\xf1\x875Q0959655BT\xf1\x890403\xf1\x82\023141600111233003142405A2252229333463100',
+      b'\xf1\x875Q0959655C \xf1\x890361\xf1\x82\0211413001112120004110415121610169112',
       b'\xf1\x875Q0959655D \xf1\x890388\xf1\x82\0211413001113120006110417121A101A9113',
       b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271112111312--071104171825102591131211',
       b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023271212111312--071104171838103891131211',
@@ -217,6 +244,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00442A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A00642A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521A07B05A1',
+      b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00521A00602A0',
       b'\xf1\x875Q0909144L \xf1\x891021\xf1\x82\00522A00402A0',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00511A00403A0',
       b'\xf1\x875Q0909144R \xf1\x891061\xf1\x82\00516A00604A1',
@@ -233,10 +261,12 @@ FW_VERSIONS = {
       b'\xf1\x875QN909144B \xf1\x895082\xf1\x82\00571A01A18A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\00101',
       b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\00101',
       b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101',
       b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\00101',
       b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101',
+      b'\xf1\x875Q0907572E \xf1\x89X310\xf1\x82\00101',
       b'\xf1\x875Q0907572F \xf1\x890400\xf1\x82\00101',
       b'\xf1\x875Q0907572G \xf1\x890571',
       b'\xf1\x875Q0907572H \xf1\x890620',
@@ -251,6 +281,7 @@ FW_VERSIONS = {
       b'\xf1\x8704E906024AS\xf1\x899912',
       b'\xf1\x8704E906024B \xf1\x895594',
       b'\xf1\x8704E906024L \xf1\x895595',
+      b'\xf1\x8704E906024L \xf1\x899970',
       b'\xf1\x8704E906027MS\xf1\x896223',
       b'\xf1\x875G0906259T \xf1\x890003',
     ],
@@ -284,20 +315,24 @@ FW_VERSIONS = {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906023AH\xf1\x893379',
       b'\xf1\x8704L906026GA\xf1\x892013',
+      b'\xf1\x8704L906026KD\xf1\x894798',
       b'\xf1\x873G0906264  \xf1\x890004',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870CW300048R \xf1\x890610',
       b'\xf1\x870D9300014L \xf1\x895002',
       b'\xf1\x870DD300045T \xf1\x891601',
+      b'\xf1\x870GC300042H \xf1\x891404',
     ],
     (Ecu.srs, 0x715, None): [
       b'\xf1\x873Q0959655AN\xf1\x890306\xf1\x82\r58160058140013036914110311',
       b'\xf1\x873Q0959655BB\xf1\x890195\xf1\x82\r56140056130012026612120211',
+      b'\xf1\x873Q0959655BK\xf1\x890703\xf1\x82\0165915005914001344701311442900',
       b'\xf1\x875Q0959655S \xf1\x890870\xf1\x82\02315120011111200631145171716121691132111',
     ],
     (Ecu.eps, 0x712, None): [
       b'\xf1\x875Q0909143M \xf1\x892041\xf1\x820522B0080803',
+      b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521B00606A1',
       b'\xf1\x875Q0909144S \xf1\x891063\xf1\x82\00516B00501A1',
       b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521B00703A1',
     ],
@@ -307,6 +342,23 @@ FW_VERSIONS = {
       b'\xf1\x875Q0907572R \xf1\x890771',
     ],
   },
+  CAR.TAOS_MK1: {
+    (Ecu.engine, 0x7e0, None): [
+        b'\xf1\x8705E906013E \xf1\x891624',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+        b'\xf1\x8709S927158BL\xf1\x893791',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+        b'\xf1\x872Q0907572T \xf1\x890383',
+    ],
+    (Ecu.eps, 0x712, None): [
+        b'\xf1\x875QM909144C \xf1\x891082\xf1\x82\x0521060605A1',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x875Q0959655CB\xf1\x890421\xf1\x82\x1311111111333500314646021450149333613100',
+    ],
+  },
   CAR.TCROSS_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704C906025AK\xf1\x897053',
@@ -373,6 +425,23 @@ FW_VERSIONS = {
       b'\xf1\x873Q0907572C \xf1\x890195',
     ],
   },
+  CAR.TRANSPORTER_T61: {
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8704L906057N \xf1\x890413',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870BT300012G \xf1\x893102',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x872Q0959655AE\xf1\x890506\xf1\x82\02316170411110411--04041704161611152S1411',
+    ],
+    (Ecu.eps, 0x712, None): [
+      b'\xf1\x877LA909144F \xf1\x897150\xf1\x82\005323A5519A2',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x872Q0907572R \xf1\x890372',
+    ],
+  },
   CAR.AUDI_A3_MK3: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906023AN\xf1\x893695',
@@ -382,12 +451,14 @@ FW_VERSIONS = {
       b'\xf1\x8704L997022N \xf1\x899459',
       b'\xf1\x875G0906259L \xf1\x890002',
       b'\xf1\x875G0906259Q \xf1\x890002',
+      b'\xf1\x878V0906259F \xf1\x890002',
       b'\xf1\x878V0906264B \xf1\x890003',
       b'\xf1\x878V0907115B \xf1\x890007',
     ],
     (Ecu.transmission, 0x7e1, None): [
       b'\xf1\x870CW300044T \xf1\x895245',
       b'\xf1\x870CW300048  \xf1\x895201',
+      b'\xf1\x870D9300012  \xf1\x894912',
       b'\xf1\x870D9300013B \xf1\x894931',
       b'\xf1\x870D9300041N \xf1\x894512',
       b'\xf1\x870DD300046A \xf1\x891602',
@@ -397,6 +468,7 @@ FW_VERSIONS = {
       b'\xf1\x870GC300042J \xf1\x891402',
     ],
     (Ecu.srs, 0x715, None): [
+      b'\xf1\x875Q0959655AB\xf1\x890388\xf1\x82\0211111001111111206110412111321139114',
       b'\xf1\x875Q0959655AM\xf1\x890315\xf1\x82\x1311111111111111311411011231129321212100',
       b'\xf1\x875Q0959655J \xf1\x890825\xf1\x82\023111112111111--171115141112221291163221',
       b'\xf1\x875Q0959655J \xf1\x890830\xf1\x82\023121111111211--261117141112231291163221',
@@ -405,6 +477,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0959655N \xf1\x890361\xf1\x82\0211212001112111104110411111521159114',
     ],
     (Ecu.eps, 0x712, None): [
+      b'\xf1\x873Q0909144H \xf1\x895061\xf1\x82\00566G0HA14A1',
       b'\xf1\x875Q0909144AB\xf1\x891082\xf1\x82\00521G0G809A1',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00303A0',
       b'\xf1\x875Q0909144P \xf1\x891043\xf1\x82\00503G00803A0',
@@ -412,6 +485,7 @@ FW_VERSIONS = {
       b'\xf1\x875Q0909144T \xf1\x891072\xf1\x82\00521G00807A1',
     ],
     (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x875Q0907567N \xf1\x890400\xf1\x82\00101',
       b'\xf1\x875Q0907572D \xf1\x890304\xf1\x82\00101',
       b'\xf1\x875Q0907572G \xf1\x890571',
       b'\xf1\x875Q0907572H \xf1\x890620',
@@ -479,6 +553,40 @@ FW_VERSIONS = {
       b'\xf1\x875Q0907572P \xf1\x890682',
     ],
   },
+  CAR.SKODA_KAMIQ_MK1: {
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8705C906032M \xf1\x891333',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870CW300020  \xf1\x891906',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x872Q0959655AM\xf1\x890351\xf1\x82\0222221042111042121040404042E2711152H14',
+    ],
+    (Ecu.eps, 0x712, None): [
+      b'\xf1\x872Q1909144M \xf1\x896041',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x872Q0907572T \xf1\x890383',
+    ],
+  },
+  CAR.SKODA_KAROQ_MK1: {
+    (Ecu.engine, 0x7e0, None): [
+      b'\xf1\x8705E906018P \xf1\x896020',
+    ],
+    (Ecu.transmission, 0x7e1, None): [
+      b'\xf1\x870CW300041S \xf1\x891615',
+    ],
+    (Ecu.srs, 0x715, None): [
+      b'\xf1\x873Q0959655BH\xf1\x890712\xf1\x82\0161213001211001101131122012100',
+    ],
+    (Ecu.eps, 0x712, None): [
+      b'\xf1\x875Q0910143C \xf1\x892211\xf1\x82\00567T6100500',
+    ],
+    (Ecu.fwdRadar, 0x757, None): [
+      b'\xf1\x872Q0907572M \xf1\x890233',
+    ],
+  },
   CAR.SKODA_KODIAQ_MK1: {
     (Ecu.engine, 0x7e0, None): [
       b'\xf1\x8704E906027DD\xf1\x893123',
diff --git a/selfdrive/car/volkswagen/volkswagencan.py b/selfdrive/car/volkswagen/volkswagencan.py
index 0e35d21d6..10e0054c7 100644
--- a/selfdrive/car/volkswagen/volkswagencan.py
+++ b/selfdrive/car/volkswagen/volkswagencan.py
@@ -16,26 +16,20 @@ def create_mqb_steering_control(packer, bus, apply_steer, idx, lkas_enabled):
   return packer.make_can_msg("HCA_01", bus, values, idx)
 
 def create_mqb_hud_control(packer, bus, enabled, steering_pressed, hud_alert, left_lane_visible, right_lane_visible,
-                           ldw_lane_warning_left, ldw_lane_warning_right, ldw_side_dlc_tlc, ldw_dlc, ldw_tlc,
-                           standstill, left_lane_depart, right_lane_depart):
+                           ldw_stock_values, left_lane_depart, right_lane_depart):
   # Lane color reference:
-  # 0 (LKAS disabled) - off 
-  # 1 (LKAS enabled, no lane detected) - dark gray 
-  # 2 (LKAS enabled, lane detected) - light gray on VW, green or white on Audi depending on year or virtual cockpit.  On a color MFD on a 2015 A3 TDI it is white, virtual cockpit on a 2018 A3 e-Tron its green. 
-  # 3 (LKAS enabled, lane departure detected) - white on VW, red on Audi 
-
-  values = {
+  # 0 (LKAS disabled) - off
+  # 1 (LKAS enabled, no lane detected) - dark gray
+  # 2 (LKAS enabled, lane detected) - light gray on VW, green or white on Audi depending on year or virtual cockpit.  On a color MFD on a 2015 A3 TDI it is white, virtual cockpit on a 2018 A3 e-Tron its green.
+  # 3 (LKAS enabled, lane departure detected) - white on VW, red on Audi
+  values = ldw_stock_values.copy()
+  values.update({
     "LDW_Status_LED_gelb": 1 if enabled and steering_pressed else 0,
     "LDW_Status_LED_gruen": 1 if enabled and not steering_pressed else 0,
     "LDW_Lernmodus_links": 3 if left_lane_depart else 1 + left_lane_visible,
     "LDW_Lernmodus_rechts": 3 if right_lane_depart else 1 + right_lane_visible,
     "LDW_Texte": hud_alert,
-    "LDW_SW_Warnung_links": ldw_lane_warning_left,
-    "LDW_SW_Warnung_rechts": ldw_lane_warning_right,
-    "LDW_Seite_DLCTLC": ldw_side_dlc_tlc,
-    "LDW_DLC": ldw_dlc,
-    "LDW_TLC": ldw_tlc
-  }
+  })
   return packer.make_can_msg("LDW_02", bus, values)
 
 def create_mqb_acc_buttons_control(packer, bus, buttonStatesToSend, CS, idx):
diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript
index 57c789479..9b496fef7 100644
--- a/selfdrive/common/SConscript
+++ b/selfdrive/common/SConscript
@@ -18,15 +18,10 @@ _common = fxn('common', common_libs, LIBS="json11")
 
 files = [
   'clutil.cc',
-  'glutil.cc',
   'visionimg.cc',
 ]
 
 if arch == "aarch64":
-  files += [
-    'framebuffer.cc',
-    'touch.c',
-  ]
   _gpu_libs = ['gui', 'adreno_utils']
 elif arch == "larch64":
   _gpu_libs = ["GLESv2"]
diff --git a/selfdrive/common/framebuffer.cc b/selfdrive/common/framebuffer.cc
deleted file mode 100644
index 79051f7e8..000000000
--- a/selfdrive/common/framebuffer.cc
+++ /dev/null
@@ -1,146 +0,0 @@
-#include "selfdrive/common/framebuffer.h"
-
-#include <cstdio>
-#include <cassert>
-
-#include "selfdrive/common/util.h"
-
-#include <ui/DisplayInfo.h>
-
-#include <gui/ISurfaceComposer.h>
-#include <gui/Surface.h>
-#include <gui/SurfaceComposerClient.h>
-#include <GLES2/gl2.h>
-#include <EGL/eglext.h>
-
-#define BACKLIGHT_LEVEL 205
-
-using namespace android;
-
-struct FramebufferState {
-    sp<SurfaceComposerClient> session;
-    sp<IBinder> dtoken;
-    DisplayInfo dinfo;
-    sp<SurfaceControl> control;
-
-    sp<Surface> s;
-    EGLDisplay display;
-
-    EGLint egl_major, egl_minor;
-    EGLConfig config;
-    EGLSurface surface;
-    EGLContext context;
-};
-
-void FrameBuffer::swap() {
-  eglSwapBuffers(s->display, s->surface);
-  assert(glGetError() == GL_NO_ERROR);
-}
-
-bool set_brightness(int brightness) {
-  char bright[64];
-  snprintf(bright, sizeof(bright), "%d", brightness);
-  return 0 == util::write_file("/sys/class/leds/lcd-backlight/brightness", bright, strlen(bright));
-}
-
-void FrameBuffer::set_power(int mode) {
-  SurfaceComposerClient::setDisplayPowerMode(s->dtoken, mode);
-}
-
-FrameBuffer::FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h) {
-  s = new FramebufferState;
-
-  s->session = new SurfaceComposerClient();
-  assert(s->session != NULL);
-
-  s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
-                ISurfaceComposer::eDisplayIdMain);
-  assert(s->dtoken != NULL);
-
-  status_t status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
-  assert(status == 0);
-
-  //int orientation = 3; // rotate framebuffer 270 degrees
-  int orientation = 1; // rotate framebuffer 90 degrees
-  if(orientation == 1 || orientation == 3) {
-      int temp = s->dinfo.h;
-      s->dinfo.h = s->dinfo.w;
-      s->dinfo.w = temp;
-  }
-
-  printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
-
-  Rect destRect(s->dinfo.w, s->dinfo.h);
-  s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
-
-  s->control = s->session->createSurface(String8(name),
-                  s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
-  assert(s->control != NULL);
-
-  SurfaceComposerClient::openGlobalTransaction();
-  status = s->control->setLayer(layer);
-  SurfaceComposerClient::closeGlobalTransaction();
-  assert(status == 0);
-
-  s->s = s->control->getSurface();
-  assert(s->s != NULL);
-
-  // init opengl and egl
-  const EGLint attribs[] = {
-    EGL_RED_SIZE,     8,
-    EGL_GREEN_SIZE,   8,
-    EGL_BLUE_SIZE,    8,
-    EGL_ALPHA_SIZE,   alpha ? 8 : 0,
-    EGL_DEPTH_SIZE,   0,
-    EGL_STENCIL_SIZE, 8,
-    EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
-    // enable MSAA
-    EGL_SAMPLE_BUFFERS, 1,
-    EGL_SAMPLES, 4,
-    EGL_NONE,
-  };
-
-  s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
-  assert(s->display != EGL_NO_DISPLAY);
-
-  int success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
-  assert(success);
-
-  printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
-
-  EGLint num_configs;
-  success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
-  assert(success);
-
-  s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
-  assert(s->surface != EGL_NO_SURFACE);
-
-  const EGLint context_attribs[] = {
-    EGL_CONTEXT_CLIENT_VERSION, 3,
-    EGL_NONE,
-  };
-  s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
-  assert(s->context != EGL_NO_CONTEXT);
-
-  EGLint w, h;
-  eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
-  eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
-  printf("egl w %d h %d\n", w, h);
-
-  success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
-  assert(success);
-
-  printf("gl version %s\n", glGetString(GL_VERSION));
-
-  set_brightness(BACKLIGHT_LEVEL);
-
-  if (out_w) *out_w = w;
-  if (out_h) *out_h = h;
-}
-
-FrameBuffer::~FrameBuffer() {
-  eglDestroyContext(s->display, s->context);
-  eglDestroySurface(s->display, s->surface);
-  eglTerminate(s->display);
-  delete s;
-}
diff --git a/selfdrive/common/framebuffer.h b/selfdrive/common/framebuffer.h
deleted file mode 100644
index d7054374c..000000000
--- a/selfdrive/common/framebuffer.h
+++ /dev/null
@@ -1,18 +0,0 @@
-#pragma once
-
-#include <cstdlib>
-
-#include "hardware/hwcomposer_defs.h"
-
-bool set_brightness(int brightness);
-
-struct FramebufferState;
-class FrameBuffer {
- public:
-  FrameBuffer(const char *name, uint32_t layer, int alpha, int *out_w, int *out_h);
-  ~FrameBuffer();
-  void set_power(int mode);
-  void swap();
-private:
-  FramebufferState *s;
-};
diff --git a/selfdrive/common/glutil.cc b/selfdrive/common/glutil.cc
deleted file mode 100644
index f13e132a8..000000000
--- a/selfdrive/common/glutil.cc
+++ /dev/null
@@ -1,63 +0,0 @@
-#include "selfdrive/common/glutil.h"
-
-#include <cassert>
-#include <cstdio>
-#include <cstdlib>
-#include <string>
-
-static GLuint load_shader(GLenum shaderType, const char *src) {
-  GLint status = 0, len = 0;
-  GLuint shader = glCreateShader(shaderType);
-  assert(shader != 0);
-
-  glShaderSource(shader, 1, &src, NULL);
-  glCompileShader(shader);
-  glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
-  if (!status) {
-    glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &len);
-    if (len) {
-      std::string msg(len, '\0');
-      glGetShaderInfoLog(shader, len, NULL, msg.data());
-      fprintf(stderr, "error compiling shader:\n%s\n", msg.c_str());
-    }
-    assert(0);
-  }
-  return shader;
-}
-
-GLShader::GLShader(const char *vert_src, const char *frag_src) {
-  GLint status = 0, len = 0;
-  prog = glCreateProgram();
-  assert(prog != 0);
-
-  vert = load_shader(GL_VERTEX_SHADER, vert_src);
-  frag = load_shader(GL_FRAGMENT_SHADER, frag_src);
-  glAttachShader(prog, vert);
-  glAttachShader(prog, frag);
-  glLinkProgram(prog);
-
-  glGetProgramiv(prog, GL_LINK_STATUS, &status);
-  if (!status) {
-    glGetProgramiv(prog, GL_INFO_LOG_LENGTH, &len);
-    if (len) {
-      std::string msg(len, '\0');
-      glGetProgramInfoLog(prog, len, NULL, msg.data());
-      fprintf(stderr, "error linking program:\n%s\n", msg.c_str());
-    }
-    assert(0);
-  }
-}
-
-GLShader::~GLShader() {
-  glDeleteProgram(prog);
-  glDeleteShader(frag);
-  glDeleteShader(vert);
-}
-
-GLuint GLShader::getUniformLocation(const char *name) {
-  auto it = uniform_loc_map.find(name);
-  if (it == uniform_loc_map.end()) {
-    it = uniform_loc_map.insert(it, {name, glGetUniformLocation(prog, name)});
-  }
-  return it->second;
-}
diff --git a/selfdrive/common/glutil.h b/selfdrive/common/glutil.h
deleted file mode 100644
index 7180f855c..000000000
--- a/selfdrive/common/glutil.h
+++ /dev/null
@@ -1,21 +0,0 @@
-#pragma once
-
-#include <map>
-
-#ifdef __APPLE__
-#include <OpenGL/gl3.h>
-#else
-#include <GLES3/gl3.h>
-#endif
-
-class GLShader {
-public:
-  GLShader(const char *vert_src, const char *frag_src);
-  ~GLShader();
-  GLuint getUniformLocation(const char * name);
-  GLuint prog = 0;
-
-private:
-  GLuint vert = 0, frag = 0;
-  std::map<const char*, GLint> uniform_loc_map;
-};
diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index e68bf1883..7aec15e7b 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,11 +1,23 @@
 #pragma once
+
+#include <array>
+
 const int  TRAJECTORY_SIZE = 33;
 const int LAT_MPC_N = 16;
 const int LON_MPC_N = 32;
 const float MIN_DRAW_DISTANCE = 10.0;
 const float MAX_DRAW_DISTANCE = 100.0;
 
-const double T_IDXS[TRAJECTORY_SIZE] = {
+template<typename T_SRC, typename  T_DST, size_t size>
+const std::array<T_DST, size> convert_array_to_type(const std::array<T_SRC, size> &src) {
+  std::array<T_DST, size> dst = {};
+  for (int i=0; i<size; i++) {
+    dst[i] = src[i];
+  }
+  return dst;
+}
+
+const std::array<double, TRAJECTORY_SIZE> T_IDXS = {
         0.        ,  0.00976562,  0.0390625 ,  0.08789062,  0.15625   ,
         0.24414062,  0.3515625 ,  0.47851562,  0.625     ,  0.79101562,
         0.9765625 ,  1.18164062,  1.40625   ,  1.65039062,  1.9140625 ,
@@ -13,25 +25,28 @@ const double T_IDXS[TRAJECTORY_SIZE] = {
         3.90625   ,  4.30664062,  4.7265625 ,  5.16601562,  5.625     ,
         6.10351562,  6.6015625 ,  7.11914062,  7.65625   ,  8.21289062,
         8.7890625 ,  9.38476562, 10.};
-const double X_IDXS[TRAJECTORY_SIZE] = {
+const auto T_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(T_IDXS);
+
+const std::array<double, TRAJECTORY_SIZE> X_IDXS = {
          0.    ,   0.1875,   0.75  ,   1.6875,   3.    ,   4.6875,
          6.75  ,   9.1875,  12.    ,  15.1875,  18.75  ,  22.6875,
         27.    ,  31.6875,  36.75  ,  42.1875,  48.    ,  54.1875,
         60.75  ,  67.6875,  75.    ,  82.6875,  90.75  ,  99.1875,
        108.    , 117.1875, 126.75  , 136.6875, 147.    , 157.6875,
        168.75  , 180.1875, 192.};
+const auto X_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(X_IDXS);
 
 #ifdef __cplusplus
 
 #include "selfdrive/common/mat.h"
 #include "selfdrive/hardware/hw.h"
 const mat3 fcam_intrinsic_matrix =
-    Hardware::TICI() ? (mat3){{2648.0, 0.0, 1928.0 / 2,
-                               0.0, 2648.0, 1208.0 / 2,
-                               0.0, 0.0, 1.0}}
-                     : (mat3){{910., 0., 1164.0 / 2,
-                               0., 910., 874.0 / 2,
-                               0., 0., 1.}};
+    Hardware::EON() ? (mat3){{910., 0., 1164.0 / 2,
+                              0., 910., 874.0 / 2,
+                              0., 0., 1.}}
+                    : (mat3){{2648.0, 0.0, 1928.0 / 2,
+                              0.0, 2648.0, 1208.0 / 2,
+                              0.0, 0.0, 1.0}};
 
 // without unwarp, focal length is for center portion only
 const mat3 ecam_intrinsic_matrix = (mat3){{620.0, 0.0, 1928.0 / 2,
@@ -39,7 +54,7 @@ const mat3 ecam_intrinsic_matrix = (mat3){{620.0, 0.0, 1928.0 / 2,
                                            0.0, 0.0, 1.0}};
 
 static inline mat3 get_model_yuv_transform(bool bayer = true) {
-  float db_s = Hardware::TICI() ? 1.0 : 0.5; // debayering does a 2x downscale on EON
+  float db_s = Hardware::EON() ? 0.5 : 1.0; // debayering does a 2x downscale on EON
   const mat3 transform = (mat3){{
     1.0, 0.0, 0.0,
     0.0, 1.0, 0.0,
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index b261855b4..22926c593 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -123,38 +123,39 @@ private:
 
 std::unordered_map<std::string, uint32_t> keys = {
     {"AccessToken", CLEAR_ON_MANAGER_START | DONT_LOG},
-    {"ApiCache_DriveStats", PERSISTENT},
-    {"ApiCache_Device", PERSISTENT},
-    {"ApiCache_Owner", PERSISTENT},
-    {"ApiCache_NavDestinations", PERSISTENT},
     {"AthenadPid", PERSISTENT},
+    {"BootedOnroad", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"CalibrationParams", PERSISTENT},
     {"CarBatteryCapacity", PERSISTENT},
     {"CarParams", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
     {"CarParamsCache", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"CarVin", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
     {"CommunityFeaturesToggle", PERSISTENT},
+    {"CompletedTrainingVersion", PERSISTENT},
     {"ControlsReady", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT | CLEAR_ON_IGNITION_ON},
     {"CurrentRoute", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_ON},
-    {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
-    {"EndToEndToggle", PERSISTENT},
-    {"CompletedTrainingVersion", PERSISTENT},
     {"DisablePowerDown", PERSISTENT},
+    {"DisableRadar_Allow", PERSISTENT},
+    {"DisableRadar", PERSISTENT}, // WARNING: THIS DISABLES AEB
     {"DisableUpdates", PERSISTENT},
-    {"EnableWideCamera", CLEAR_ON_MANAGER_START},
-    {"DoUninstall", CLEAR_ON_MANAGER_START},
     {"DongleId", PERSISTENT},
-    {"GitDiff", PERSISTENT},
+    {"DoUninstall", CLEAR_ON_MANAGER_START},
+    {"EnableWideCamera", CLEAR_ON_MANAGER_START},
+    {"EndToEndToggle", PERSISTENT},
+    {"ForcePowerDown", CLEAR_ON_MANAGER_START},
     {"GitBranch", PERSISTENT},
     {"GitCommit", PERSISTENT},
-    {"GitRemote", PERSISTENT},
+    {"GitDiff", PERSISTENT},
     {"GithubSshKeys", PERSISTENT},
     {"GithubUsername", PERSISTENT},
+    {"GitRemote", PERSISTENT},
+    {"GsmApn", PERSISTENT},
     {"GsmRoaming", PERSISTENT},
     {"HardwareSerial", PERSISTENT},
     {"HasAcceptedTerms", PERSISTENT},
-    {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
     {"IMEI", PERSISTENT},
+    {"InstallDate", PERSISTENT},
+    {"IsDriverViewEnabled", CLEAR_ON_MANAGER_START},
     {"IsLdwEnabled", PERSISTENT},
     {"IsMetric", PERSISTENT},
     {"IsOffroad", CLEAR_ON_MANAGER_START},
@@ -162,7 +163,7 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"IsRHD", PERSISTENT},
     {"IsTakingSnapshot", CLEAR_ON_MANAGER_START},
     {"IsUpdateAvailable", CLEAR_ON_MANAGER_START},
-    {"UploadRaw", PERSISTENT},
+    {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
     {"LastGPSPosition", PERSISTENT},
     {"LastUpdateException", PERSISTENT},
@@ -172,9 +173,9 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"NavSettingTime24h", PERSISTENT},
     {"OpenpilotEnabledToggle", PERSISTENT},
+    {"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"PandaFirmware", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"PandaFirmwareHex", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
-    {"PandaDongleId", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
     {"Passive", PERSISTENT},
     {"PrimeRedirected", PERSISTENT},
@@ -182,29 +183,32 @@ std::unordered_map<std::string, uint32_t> keys = {
     {"RecordFrontLock", PERSISTENT},  // for the internal fleet
     {"ReleaseNotes", PERSISTENT},
     {"ShouldDoUpdate", CLEAR_ON_MANAGER_START},
-    {"SubscriberInfo", PERSISTENT},
     {"SshEnabled", PERSISTENT},
+    {"SubscriberInfo", PERSISTENT},
     {"TermsVersion", PERSISTENT},
     {"Timezone", PERSISTENT},
     {"TrainingVersion", PERSISTENT},
     {"UpdateAvailable", CLEAR_ON_MANAGER_START},
     {"UpdateFailedCount", CLEAR_ON_MANAGER_START},
+    {"UploadRaw", PERSISTENT},
     {"Version", PERSISTENT},
     {"VisionRadarToggle", PERSISTENT},
+    {"ApiCache_Device", PERSISTENT},
+    {"ApiCache_DriveStats", PERSISTENT},
+    {"ApiCache_NavDestinations", PERSISTENT},
+    {"ApiCache_Owner", PERSISTENT},
     {"Offroad_ChargeDisabled", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
     {"Offroad_ConnectivityNeeded", CLEAR_ON_MANAGER_START},
     {"Offroad_ConnectivityNeededPrompt", CLEAR_ON_MANAGER_START},
-    {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
-    {"Offroad_PandaFirmwareMismatch", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
+    {"Offroad_HardwareUnsupported", CLEAR_ON_MANAGER_START},
     {"Offroad_InvalidTime", CLEAR_ON_MANAGER_START},
     {"Offroad_IsTakingSnapshot", CLEAR_ON_MANAGER_START},
     {"Offroad_NeosUpdate", CLEAR_ON_MANAGER_START},
-    {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
-    {"Offroad_HardwareUnsupported", CLEAR_ON_MANAGER_START},
-    {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
     {"Offroad_NvmeMissing", CLEAR_ON_MANAGER_START},
-    {"ForcePowerDown", CLEAR_ON_MANAGER_START},
-    {"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
+    {"Offroad_PandaFirmwareMismatch", CLEAR_ON_MANAGER_START | CLEAR_ON_PANDA_DISCONNECT},
+    {"Offroad_TemperatureTooHigh", CLEAR_ON_MANAGER_START},
+    {"Offroad_UnofficialHardware", CLEAR_ON_MANAGER_START},
+    {"Offroad_UpdateFailed", CLEAR_ON_MANAGER_START},
 };
 
 } // namespace
diff --git a/selfdrive/common/params.h b/selfdrive/common/params.h
index 2bb636dbd..77d00b76e 100644
--- a/selfdrive/common/params.h
+++ b/selfdrive/common/params.h
@@ -3,6 +3,7 @@
 #include <map>
 #include <sstream>
 #include <string>
+#include <optional>
 
 enum ParamKeyType {
   PERSISTENT = 0x02,
diff --git a/selfdrive/common/touch.c b/selfdrive/common/touch.c
deleted file mode 100644
index eef83d8f2..000000000
--- a/selfdrive/common/touch.c
+++ /dev/null
@@ -1,96 +0,0 @@
-#include "selfdrive/common/touch.h"
-
-#include <assert.h>
-#include <dirent.h>
-#include <fcntl.h>
-#include <linux/input.h>
-#include <stdbool.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <sys/poll.h>
-#include <unistd.h>
-
-/* this macro is used to tell if "bit" is set in "array"
- * it selects a byte from the array, and does a boolean AND
- * operation with a byte that only has the relevant bit set.
- * eg. to check for the 12th bit, we do (array[1] & 1<<4)
- */
-#define test_bit(bit, array)    (array[bit/8] & (1<<(bit%8)))
-
-static int find_dev() {
-  int err;
-
-  int ret = -1;
-
-  DIR *dir = opendir("/dev/input");
-  assert(dir);
-  struct dirent* de = NULL;
-  while ((de = readdir(dir))) {
-    if (strncmp(de->d_name, "event", 5)) continue;
-
-    int fd = openat(dirfd(dir), de->d_name, O_RDONLY);
-    assert(fd >= 0);
-
-    unsigned char ev_bits[KEY_MAX / 8 + 1];
-    err = ioctl(fd, EVIOCGBIT(EV_ABS, sizeof(ev_bits)), ev_bits);
-    assert(err >= 0);
-
-    if (test_bit(ABS_MT_POSITION_X, ev_bits) && test_bit(ABS_MT_POSITION_Y, ev_bits)) {
-      ret = fd;
-      break;
-    }
-    close(fd);
-  }
-  closedir(dir);
-
-  return ret;
-}
-
-void touch_init(TouchState *s) {
-  s->fd = find_dev();
-  assert(s->fd >= 0);
-}
-
-int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
-  assert(out_x && out_y);
-  bool up = false;
-  while (true) {
-    struct pollfd polls[] = {{
-      .fd = s->fd,
-      .events = POLLIN,
-    }};
-    int err = poll(polls, 1, timeout);
-    if (err < 0) {
-      return -1;
-    }
-    if (!(polls[0].revents & POLLIN)) {
-      break;
-    }
-
-    struct input_event event;
-    err = read(polls[0].fd, &event, sizeof(event));
-    if (err < sizeof(event)) {
-      return -1;
-    }
-
-    switch (event.type) {
-    case EV_ABS:
-      if (event.code == ABS_MT_POSITION_X) {
-        s->last_x = event.value;
-      } else if (event.code == ABS_MT_POSITION_Y) {
-        s->last_y = event.value;
-      } else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
-        up = true;
-      }
-      break;
-    default:
-      break;
-    }
-  }
-  if (up) {
-    // adjust for flippening
-    *out_x = s->last_y;
-    *out_y = 1080 - s->last_x;
-  }
-  return up;
-}
diff --git a/selfdrive/common/touch.h b/selfdrive/common/touch.h
deleted file mode 100644
index fa0faa271..000000000
--- a/selfdrive/common/touch.h
+++ /dev/null
@@ -1,17 +0,0 @@
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-typedef struct TouchState {
-  int fd;
-  int last_x, last_y;
-} TouchState;
-
-void touch_init(TouchState *s);
-int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout);
-
-#ifdef __cplusplus
-}
-#endif
diff --git a/selfdrive/common/util.cc b/selfdrive/common/util.cc
index 99cf2ae88..cd00b626b 100644
--- a/selfdrive/common/util.cc
+++ b/selfdrive/common/util.cc
@@ -40,14 +40,16 @@ int set_realtime_priority(int level) {
 #endif
 }
 
-int set_core_affinity(int core) {
+int set_core_affinity(std::vector<int> cores) {
 #ifdef __linux__
   long tid = syscall(SYS_gettid);
-  cpu_set_t rt_cpu;
+  cpu_set_t cpu;
 
-  CPU_ZERO(&rt_cpu);
-  CPU_SET(core, &rt_cpu);
-  return sched_setaffinity(tid, sizeof(rt_cpu), &rt_cpu);
+  CPU_ZERO(&cpu);
+  for (const int n : cores) {
+    CPU_SET(n, &cpu);
+  }
+  return sched_setaffinity(tid, sizeof(cpu), &cpu);
 #else
   return -1;
 #endif
diff --git a/selfdrive/common/util.h b/selfdrive/common/util.h
index 987bf95a9..d3d2a2e43 100644
--- a/selfdrive/common/util.h
+++ b/selfdrive/common/util.h
@@ -12,6 +12,7 @@
 #include <memory>
 #include <string>
 #include <thread>
+#include <vector>
 
 // keep trying if x gets interrupted by a signal
 #define HANDLE_EINTR(x)                                       \
@@ -31,7 +32,7 @@ typedef void (*sighandler_t)(int sig);
 void set_thread_name(const char* name);
 
 int set_realtime_priority(int level);
-int set_core_affinity(int core);
+int set_core_affinity(std::vector<int> cores);
 
 namespace util {
 
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index fecce3335..8869dfe97 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.8.9"
+#define COMMA_VERSION "0.8.10"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index e2737ee05..fd43d65b0 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -16,7 +16,7 @@ from selfdrive.car.car_helpers import get_car, get_startup_event, get_one_can
 from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET
 from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise
 from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature
-from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED
+from selfdrive.controls.lib.longcontrol import LongControl
 from selfdrive.controls.lib.latcontrol_pid import LatControlPID
 from selfdrive.controls.lib.latcontrol_indi import LatControlINDI
 from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
@@ -48,6 +48,8 @@ Desire = log.LateralPlan.Desire
 LaneChangeState = log.LateralPlan.LaneChangeState
 LaneChangeDirection = log.LateralPlan.LaneChangeDirection
 EventName = car.CarEvent.EventName
+ButtonEvent = car.CarState.ButtonEvent
+SafetyModel = car.CarParams.SafetyModel
 
 
 class Controls:
@@ -71,7 +73,7 @@ class Controls:
     self.sm = sm
     if self.sm is None:
       ignore = ['driverCameraState', 'managerState'] if SIMULATION else None
-      self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration',
+      self.sm = messaging.SubMaster(['deviceState', 'pandaStates', 'peripheralState', 'modelV2', 'liveCalibration',
                                      'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
                                      'managerState', 'liveParameters', 'radarState'] + self.camera_packets + joystick_packet,
                                      ignore_alive=ignore, ignore_avg_freq=['radarState', 'longitudinalPlan'])
@@ -109,7 +111,9 @@ class Controls:
     self.read_only = not car_recognized or not controller_available or \
                        self.CP.dashcamOnly or community_feature_disallowed
     if self.read_only:
-      self.CP.safetyModel = car.CarParams.SafetyModel.noOutput
+      safety_config = car.CarParams.SafetyConfig.new_message()
+      safety_config.safetyModel = car.CarParams.SafetyModel.noOutput
+      self.CP.safetyConfigs = [safety_config]
 
     # Write CarParams for radard
     cp_bytes = self.CP.to_bytes()
@@ -126,7 +130,7 @@ class Controls:
     if self.CP.steerControlType == car.CarParams.SteerControlType.angle:
       self.LaC = LatControlAngle(self.CP)
     elif self.CP.lateralTuning.which() == 'pid':
-      self.LaC = LatControlPID(self.CP)
+      self.LaC = LatControlPID(self.CP, self.CI)
     elif self.CP.lateralTuning.which() == 'indi':
       self.LaC = LatControlINDI(self.CP)
     elif self.CP.lateralTuning.which() == 'lqr':
@@ -149,14 +153,12 @@ class Controls:
     self.events_prev = []
     self.current_alert_types = [ET.PERMANENT]
     self.logged_comm_issue = False
-    self.v_target = 0.0
-    self.a_target = 0.0
+    self.button_timers = {ButtonEvent.Type.decelCruise: 0, ButtonEvent.Type.accelCruise: 0}
 
     # TODO: no longer necessary, aside from process replay
     self.sm['liveParameters'].valid = True
 
-    self.startup_event = get_startup_event(car_recognized, controller_available, self.CP.fuzzyFingerprint,
-                                           len(self.CP.carFw) > 0)
+    self.startup_event = get_startup_event(car_recognized, controller_available, len(self.CP.carFw) > 0)
 
     if not sounds_available:
       self.events.add(EventName.soundsUnavailable, static=True)
@@ -192,7 +194,8 @@ class Controls:
       return
 
     # Create events for battery, temperature, disk space, and memory
-    if EON and self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
+    if EON and (self.sm['peripheralState'].pandaType != PandaType.uno) and \
+       self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
       # at zero percent battery, while discharging, OP should not allowed
       self.events.add(EventName.lowBattery)
     if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
@@ -203,13 +206,15 @@ class Controls:
     # TODO: make tici threshold the same
     if self.sm['deviceState'].memoryUsagePercent > (90 if TICI else 65) and not SIMULATION:
       self.events.add(EventName.lowMemory)
-    cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
-    if max(cpus, default=0) > 95 and not SIMULATION:
-      self.events.add(EventName.highCpuUsage)
+
+    # TODO: enable this once loggerd CPU usage is more reasonable
+    #cpus = list(self.sm['deviceState'].cpuUsagePercent)[:(-1 if EON else None)]
+    #if max(cpus, default=0) > 95 and not SIMULATION:
+    #  self.events.add(EventName.highCpuUsage)
 
     # Alert if fan isn't spinning for 5 seconds
-    if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
-      if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
+    if self.sm['peripheralState'].pandaType in [PandaType.uno, PandaType.dos]:
+      if self.sm['peripheralState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
         if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
           self.events.add(EventName.fanMalfunction)
       else:
@@ -241,16 +246,21 @@ class Controls:
     if self.can_rcv_error or not CS.canValid:
       self.events.add(EventName.canError)
 
-    safety_mismatch = self.sm['pandaState'].safetyModel != self.CP.safetyModel or self.sm['pandaState'].safetyParam != self.CP.safetyParam
-    if safety_mismatch or self.mismatch_counter >= 200:
-      self.events.add(EventName.controlsMismatch)
+    for i, pandaState in enumerate(self.sm['pandaStates']):
+      # All pandas must match the list of safetyConfigs, and if outside this list, must be silent
+      if i < len(self.CP.safetyConfigs):
+        safety_mismatch = pandaState.safetyModel != self.CP.safetyConfigs[i].safetyModel or pandaState.safetyParam != self.CP.safetyConfigs[i].safetyParam
+      else:
+        safety_mismatch = pandaState.safetyModel != SafetyModel.silent
+      if safety_mismatch or self.mismatch_counter >= 200:
+        self.events.add(EventName.controlsMismatch)
 
     if not self.sm['liveParameters'].valid:
       self.events.add(EventName.vehicleModelInvalid)
 
     if len(self.sm['radarState'].radarErrors):
       self.events.add(EventName.radarFault)
-    elif not self.sm.valid["pandaState"]:
+    elif not self.sm.valid["pandaStates"]:
       self.events.add(EventName.usbError)
     elif not self.sm.all_alive_and_valid():
       self.events.add(EventName.commIssue)
@@ -271,9 +281,14 @@ class Controls:
       self.events.add(EventName.posenetInvalid)
     if not self.sm['liveLocationKalman'].deviceStable:
       self.events.add(EventName.deviceFalling)
-    if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
-      self.events.add(EventName.relayMalfunction)
-    if self.sm['longitudinalPlan'].fcw or (self.enabled and self.sm['modelV2'].meta.hardBrakePredicted):
+    for pandaState in self.sm['pandaStates']:
+      if log.PandaState.FaultType.relayMalfunction in pandaState.faults:
+        self.events.add(EventName.relayMalfunction)
+
+    stock_long_is_braking = self.enabled and not self.CP.openpilotLongitudinalControl and CS.aEgo < -1.5
+    model_fcw = self.sm['modelV2'].meta.hardBrakePredicted and not CS.brakePressed and not stock_long_is_braking
+    planner_fcw = self.sm['longitudinalPlan'].fcw and self.enabled
+    if planner_fcw or model_fcw:
       self.events.add(EventName.fcw)
 
     if TICI:
@@ -320,7 +335,7 @@ class Controls:
       v_future = speeds[-1]
     else:
       v_future = 100.0
-    if CS.brakePressed and v_future >= STARTING_TARGET_SPEED \
+    if CS.brakePressed and v_future >= self.CP.vEgoStarting \
       and self.CP.openpilotLongitudinalControl and CS.vEgo < 0.3:
       self.events.add(EventName.noTarget)
 
@@ -335,7 +350,8 @@ class Controls:
 
     all_valid = CS.canValid and self.sm.all_alive_and_valid()
     if not self.initialized and (all_valid or self.sm.frame * DT_CTRL > 3.5 or SIMULATION):
-      self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
+      if not self.read_only:
+        self.CI.init(self.CP, self.can_sock, self.pm.sock['sendcan'])
       self.initialized = True
       Params().put_bool("ControlsReady", True)
 
@@ -353,8 +369,10 @@ class Controls:
     if not self.enabled:
       self.mismatch_counter = 0
 
-    if not self.sm['pandaState'].controlsAllowed and self.enabled:
-      self.mismatch_counter += 1
+    # All pandas not in silent mode must have controlsAllowed when openpilot is enabled
+    for pandaState in self.sm['pandaStates']:
+      if pandaState.safetyModel != SafetyModel.silent and not pandaState.controlsAllowed and self.enabled:
+        self.mismatch_counter += 1
 
     self.distance_traveled += CS.vEgo * DT_CTRL
 
@@ -367,7 +385,7 @@ class Controls:
 
     # if stock cruise is completely disabled, then we can use our own set speed logic
     if not self.CP.pcmCruise:
-      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.enabled)
+      self.v_cruise_kph = update_v_cruise(self.v_cruise_kph, CS.buttonEvents, self.button_timers, self.enabled, self.is_metric)
     elif self.CP.pcmCruise and CS.cruiseState.enabled:
       self.v_cruise_kph = CS.cruiseState.speed * CV.MS_TO_KPH
 
@@ -464,7 +482,7 @@ class Controls:
     if not self.joystick_mode:
       # accel PID loop
       pid_accel_limits = self.CI.get_pid_accel_limits(self.CP, CS.vEgo, self.v_cruise_kph * CV.KPH_TO_MS)
-      actuators.accel, self.v_target, self.a_target = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits)
+      actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits)
 
       # Steering PID loop and lateral MPC
       desired_curvature, desired_curvature_rate = get_lag_adjusted_curvature(self.CP, CS.vEgo,
@@ -520,27 +538,28 @@ class Controls:
 
     return actuators, lac_log
 
+  def update_button_timers(self, buttonEvents):
+    # increment timer for buttons still pressed
+    for k in self.button_timers.keys():
+      if self.button_timers[k] > 0:
+        self.button_timers[k] += 1
+
+    for b in buttonEvents:
+      if b.type.raw in self.button_timers:
+        self.button_timers[b.type.raw] = 1 if b.pressed else 0
+
   def publish_logs(self, CS, start_time, actuators, lac_log):
     """Send actuators and hud commands to the car, send controlsstate and MPC logging"""
 
     CC = car.CarControl.new_message()
     CC.enabled = self.enabled
+    CC.active = self.active
     CC.actuators = actuators
 
-    CC.cruiseControl.override = True
-    CC.cruiseControl.cancel = not self.CP.pcmCruise or (not self.enabled and CS.cruiseState.enabled)
+    CC.cruiseControl.cancel = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
     if self.joystick_mode and self.sm.rcv_frame['testJoystick'] > 0 and self.sm['testJoystick'].buttons[0]:
       CC.cruiseControl.cancel = True
 
-    # TODO remove car specific stuff in controls
-    # Some override values for Honda
-    # brake discount removes a sharp nonlinearity
-    brake_discount = (1.0 - clip(-actuators.accel * (3.0/4.0), 0.0, 1.0))
-    speed_override = max(0.0, (self.LoC.v_pid + CS.cruiseState.speedOffset) * brake_discount)
-    CC.cruiseControl.speedOverride = float(speed_override if self.CP.pcmCruise else 0.0)
-    CC.cruiseControl.accelOverride = float(self.CI.calc_accel_override(CS.aEgo, self.a_target,
-                                                                       CS.vEgo, self.v_target))
-
     CC.hudControl.setSpeed = float(self.v_cruise_kph * CV.KPH_TO_MS)
     CC.hudControl.speedVisible = self.enabled
     CC.hudControl.lanesVisible = self.enabled
@@ -683,6 +702,8 @@ class Controls:
     self.publish_logs(CS, start_time, actuators, lac_log)
     self.prof.checkpoint("Sent")
 
+    self.update_button_timers(CS.buttonEvents)
+
   def controlsd_thread(self):
     while True:
       self.step()
diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json
index 09847a4ef..1c72c0bcf 100644
--- a/selfdrive/controls/lib/alerts_offroad.json
+++ b/selfdrive/controls/lib/alerts_offroad.json
@@ -37,10 +37,6 @@
     "text": "An update to your device's operating system is downloading in the background. You will be prompted to update when it's ready to install.",
     "severity": 0
   },
-  "Offroad_HardwareUnsupported": {
-    "text": "White and grey panda are unsupported. Upgrade to comma two or black panda.",
-    "severity": 0
-  },
   "Offroad_UnofficialHardware": {
     "text": "Device failed to register. It will not connect to or upload to comma.ai servers, and receives no support from comma.ai. If this is an official device, contact support@comma.ai.",
     "severity": 1
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9f554c88b..e9ceb7fd7 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -1,15 +1,15 @@
+import math
 from cereal import car
 from common.numpy_fast import clip, interp
 from common.realtime import DT_MDL
 from selfdrive.config import Conversions as CV
 from selfdrive.modeld.constants import T_IDXS
 
-
 # kph
 V_CRUISE_MAX = 135
 V_CRUISE_MIN = 8
-V_CRUISE_DELTA = 8
 V_CRUISE_ENABLE_MIN = 40
+
 LAT_MPC_N = 16
 LON_MPC_N = 32
 CONTROL_N = 17
@@ -19,6 +19,17 @@ CAR_ROTATION_RADIUS = 0.0
 MAX_CURVATURE_RATES = [0.03762194918267951, 0.003441203371932992]
 MAX_CURVATURE_RATE_SPEEDS = [0, 35]
 
+CRUISE_LONG_PRESS = 50
+CRUISE_NEAREST_FUNC = {
+  car.CarState.ButtonEvent.Type.accelCruise: math.ceil,
+  car.CarState.ButtonEvent.Type.decelCruise: math.floor,
+}
+CRUISE_INTERVAL_SIGN = {
+  car.CarState.ButtonEvent.Type.accelCruise: +1,
+  car.CarState.ButtonEvent.Type.decelCruise: -1,
+}
+
+
 class MPC_COST_LAT:
   PATH = 1.0
   HEADING = 1.0
@@ -40,16 +51,37 @@ def get_steer_max(CP, v_ego):
   return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)
 
 
-def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
+def update_v_cruise(v_cruise_kph, buttonEvents, button_timers, enabled, metric):
   # handle button presses. TODO: this should be in state_control, but a decelCruise press
   # would have the effect of both enabling and changing speed is checked after the state transition
+  if not enabled:
+    return v_cruise_kph
+
+  long_press = False
+  button_type = None
+
+  v_cruise_delta = 1 if metric else 1.6
+
   for b in buttonEvents:
-    if enabled and not b.pressed:
-      if b.type == car.CarState.ButtonEvent.Type.accelCruise:
-        v_cruise_kph += V_CRUISE_DELTA - (v_cruise_kph % V_CRUISE_DELTA)
-      elif b.type == car.CarState.ButtonEvent.Type.decelCruise:
-        v_cruise_kph -= V_CRUISE_DELTA - ((V_CRUISE_DELTA - v_cruise_kph) % V_CRUISE_DELTA)
-      v_cruise_kph = clip(v_cruise_kph, V_CRUISE_MIN, V_CRUISE_MAX)
+    if b.type.raw in button_timers and not b.pressed:
+      if button_timers[b.type.raw] > CRUISE_LONG_PRESS:
+        return v_cruise_kph # end long press
+      button_type = b.type.raw
+      break
+  else:
+    for k in button_timers.keys():
+      if button_timers[k] and button_timers[k] % CRUISE_LONG_PRESS == 0:
+        button_type = k
+        long_press = True
+        break
+
+  if button_type:
+    v_cruise_delta = v_cruise_delta * (5 if long_press else 1)
+    if long_press and v_cruise_kph % v_cruise_delta != 0: # partial interval
+      v_cruise_kph = CRUISE_NEAREST_FUNC[button_type](v_cruise_kph / v_cruise_delta) * v_cruise_delta
+    else:
+      v_cruise_kph += v_cruise_delta * CRUISE_INTERVAL_SIGN[button_type]
+    v_cruise_kph = clip(round(v_cruise_kph, 1), V_CRUISE_MIN, V_CRUISE_MAX)
 
   return v_cruise_kph
 
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index f08803212..e45c25801 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -204,7 +204,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
 
 
 def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
-  gps_integrated = sm['pandaState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos]
+  gps_integrated = sm['peripheralState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos]
   return Alert(
     "Poor GPS reception",
     "If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement",
@@ -219,14 +219,6 @@ def wrong_car_mode_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: boo
   return NoEntryAlert(text, duration_hud_alert=0.)
 
 
-def startup_fuzzy_fingerprint_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
-  return Alert(
-    "WARNING: No Exact Match on Car Model",
-    f"Closest Match: {CP.carFingerprint.title()[:40]}",
-    AlertStatus.userPrompt, AlertSize.mid,
-    Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 10.)
-
-
 def joystick_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
   axes = sm['testJoystick'].axes
   gb, steer = list(axes)[:2] if len(axes) else (0., 0.)
@@ -291,22 +283,10 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
       Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 10.),
   },
 
-  # openpilot uses the version strings from various ECUs to detect the correct car model.
-  # Usually all ECUs are recognized and an exact match to a car model can be made. Sometimes
-  # one or two ECUs have unrecognized versions, but the others are present in the database.
-  # If openpilot is confident about the match to a car model, it fingerprints anyway.
-  # In this case an alert is thrown since there is a small chance the wrong car was detected
-  # and the user should pay extra attention.
-  # This alert can be prevented by adding all ECU firmware version to openpilot:
-  # https://github.com/commaai/openpilot/wiki/Fingerprinting
-  EventName.startupFuzzyFingerprint: {
-    ET.PERMANENT: startup_fuzzy_fingerprint_alert,
-  },
-
   EventName.startupNoFw: {
     ET.PERMANENT: Alert(
       "Car Unrecognized",
-      "Check All Connections",
+      "Check comma power connections",
       AlertStatus.userPrompt, AlertSize.mid,
       Priority.LOWER, VisualAlert.none, AudibleAlert.none, 0., 0., 10.),
   },
@@ -812,7 +792,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
       "",
       AlertStatus.normal, AlertSize.full,
       Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2, creation_delay=0.5),
-    ET.IMMEDIATE_DISABLE: ImmediateDisableAlert("Reverse Gear"),
+    ET.USER_DISABLE: ImmediateDisableAlert("Reverse Gear"),
     ET.NO_ENTRY: NoEntryAlert("Reverse Gear"),
   },
 
diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py
deleted file mode 100644
index e582f54f5..000000000
--- a/selfdrive/controls/lib/fcw.py
+++ /dev/null
@@ -1,77 +0,0 @@
-import math
-from collections import defaultdict
-
-from common.numpy_fast import interp
-
-_FCW_A_ACT_V = [-3., -2.]
-_FCW_A_ACT_BP = [0., 30.]
-
-
-class FCWChecker():
-  def __init__(self):
-    self.reset_lead(0.0)
-    self.common_counters = defaultdict(lambda: 0)
-
-  def reset_lead(self, cur_time):
-    self.last_fcw_a = 0.0
-    self.v_lead_max = 0.0
-    self.lead_seen_t = cur_time
-    self.last_fcw_time = 0.0
-    self.last_min_a = 0.0
-
-    self.counters = defaultdict(lambda: 0)
-
-  @staticmethod
-  def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
-    max_ttc = 5.0
-
-    v_rel = v_ego - v_lead
-    a_rel = a_ego - a_lead
-
-    # assuming that closing gap ARel comes from lead vehicle decel,
-    # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
-    # This helps underweighting ARel when v_lead is close to zero.
-    t_decel = 2.
-    a_rel = min(a_rel, v_lead / t_decel)
-
-    # delta of the quadratic equation to solve for ttc
-    delta = v_rel**2 + 2 * x_lead * a_rel
-
-    # assign an arbitrary high ttc value if there is no solution to ttc
-    if delta < 0.1 or (math.sqrt(delta) + v_rel < 0.1):
-      ttc = max_ttc
-    else:
-      ttc = min(2 * x_lead / (math.sqrt(delta) + v_rel), max_ttc)
-    return ttc
-
-  def update(self, mpc_solution, cur_time, active, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
-    mpc_solution_a = list(mpc_solution[0].a_ego)
-
-    self.last_min_a = min(mpc_solution_a)
-    self.v_lead_max = max(self.v_lead_max, v_lead)
-
-    self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
-    self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0
-
-    if (fcw_lead > 0.99):
-      ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
-      self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
-      self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
-      self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
-      self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
-      self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
-      self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
-
-      a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
-      a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
-
-      future_fcw_allowed = all(c >= 10 for c in self.counters.values())
-      future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values())
-      future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed
-
-      if future_fcw and (self.last_fcw_time + 5.0 < cur_time):
-        self.last_fcw_time = cur_time
-        self.last_fcw_a = self.last_min_a
-        return True
-
-    return False
diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py
index df065b4a0..5062df36d 100644
--- a/selfdrive/controls/lib/latcontrol_pid.py
+++ b/selfdrive/controls/lib/latcontrol_pid.py
@@ -6,11 +6,12 @@ from cereal import log
 
 
 class LatControlPID():
-  def __init__(self, CP):
+  def __init__(self, CP, CI):
     self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV),
                             (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV),
                             k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, neg_limit=-1.0,
                             sat_limit=CP.steerLimitTimer)
+    self.get_steer_feedforward = CI.get_steer_feedforward_function()
 
   def reset(self):
     self.pid.reset()
@@ -23,7 +24,7 @@ class LatControlPID():
     angle_steers_des_no_offset = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo))
     angle_steers_des = angle_steers_des_no_offset + params.angleOffsetDeg
 
-    pid_log.angleError = angle_steers_des - CS.steeringAngleDeg 
+    pid_log.angleError = angle_steers_des - CS.steeringAngleDeg
     if CS.vEgo < 0.3 or not active:
       output_steer = 0.0
       pid_log.active = False
@@ -33,9 +34,8 @@ class LatControlPID():
       self.pid.pos_limit = steers_max
       self.pid.neg_limit = -steers_max
 
-      # TODO: feedforward something based on lat_plan.rateSteers
-      steer_feedforward = angle_steers_des_no_offset  # offset does not contribute to resistive torque
-      steer_feedforward *= CS.vEgo**2  # proportional to realigning tire momentum (~ lateral accel)
+      # offset does not contribute to resistive torque
+      steer_feedforward = self.get_steer_feedforward(angle_steers_des_no_offset, CS.vEgo)
 
       deadzone = 0.0
 
diff --git a/selfdrive/controls/lib/lateral_mpc/.gitignore b/selfdrive/controls/lib/lateral_mpc/.gitignore
deleted file mode 100644
index f61a939cd..000000000
--- a/selfdrive/controls/lib/lateral_mpc/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-generator
-lib_qp/
diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript
deleted file mode 100644
index 4812369e0..000000000
--- a/selfdrive/controls/lib/lateral_mpc/SConscript
+++ /dev/null
@@ -1,48 +0,0 @@
-Import('env', 'arch')
-
-cpp_path = [
-  "#",
-  "#selfdrive",
-  "#phonelibs/acado/include",
-  "#phonelibs/acado/include/acado",
-  "#phonelibs/qpoases/INCLUDE",
-  "#phonelibs/qpoases/INCLUDE/EXTRAS",
-  "#phonelibs/qpoases/SRC/",
-  "#phonelibs/qpoases",
-  "lib_mpc_export",
-]
-
-generated_c = [
-  'lib_mpc_export/acado_auxiliary_functions.c',
-  'lib_mpc_export/acado_qpoases_interface.cpp',
-  'lib_mpc_export/acado_integrator.c',
-  'lib_mpc_export/acado_solver.c',
-]
-
-generated_h = [
-  'lib_mpc_export/acado_common.h',
-  'lib_mpc_export/acado_auxiliary_functions.h',
-  'lib_mpc_export/acado_qpoases_interface.hpp',
-]
-
-interface_dir = Dir('lib_mpc_export')
-
-SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
-
-if GetOption('mpc_generate'):
-    generator_cpp = File('generator.cpp')
-
-    acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
-
-    generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
-                            CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
-
-    cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
-    env.Command(generated_c + generated_h, generator, cmd)
-
-
-
-mpc_files = ["lateral_mpc.c"] + generated_c
-env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
diff --git a/selfdrive/controls/lib/lateral_mpc/generator.cpp b/selfdrive/controls/lib/lateral_mpc/generator.cpp
deleted file mode 100644
index 61a0f5c70..000000000
--- a/selfdrive/controls/lib/lateral_mpc/generator.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-#include <math.h>
-#include <acado_code_generation.hpp>
-#include "selfdrive/common/modeldata.h"
-
-#define deg2rad(d) (d/180.0*M_PI)
-
-using namespace std;
-
-int main( )
-{
-  USING_NAMESPACE_ACADO
-
-
-  DifferentialEquation f;
-
-  DifferentialState xx; // x position
-  DifferentialState yy; // y position
-  DifferentialState psi; // vehicle heading
-  DifferentialState curvature;
-
-  OnlineData v_ego;
-  OnlineData rotation_radius;
-
-  Control curvature_rate;
-
-
-  // Equations of motion
-  f << dot(xx) == v_ego * cos(psi);
-  f << dot(yy) == v_ego * sin(psi);
-  // disable rotation radius for now
-  //f << dot(xx) == v_ego * cos(psi) - rotation_radius * sin(psi) * (v_ego * curvature);
-  //f << dot(yy) == v_ego * sin(psi) + rotation_radius * cos(psi) * (v_ego * curvature);
-  f << dot(psi) == v_ego * curvature;
-  f << dot(curvature) == curvature_rate;
-
-  // Running cost
-  Function h;
-
-  // Distance errors
-  h << yy;
-
-  // Heading error
-  h << (v_ego + 5.0 ) * psi;
-
-  // Angular rate error
-  h << (v_ego + 5.0) * 4 * curvature_rate;
-
-  BMatrix Q(3,3); Q.setAll(true);
-  // Q(0,0) = 1.0;
-  // Q(1,1) = 1.0;
-  // Q(2,2) = 1.0;
-  // Q(3,3) = 1.0;
-  // Q(4,4) = 2.0;
-
-  // Terminal cost
-  Function hN;
-
-  // Distance errors
-  hN << yy;
-
-  // Heading errors
-  hN << (2.0 * v_ego + 5.0 ) * psi;
-
-  BMatrix QN(2,2); QN.setAll(true);
-  // QN(0,0) = 1.0;
-  // QN(1,1) = 1.0;
-  // QN(2,2) = 1.0;
-  // QN(3,3) = 1.0;
-
-  double T_IDXS_ARR[LAT_MPC_N + 1];
-  memcpy(T_IDXS_ARR, T_IDXS, (LAT_MPC_N + 1) * sizeof(double));
-  Grid times(LAT_MPC_N + 1, T_IDXS_ARR);
-  OCP ocp(times);
-  ocp.subjectTo(f);
-
-  ocp.minimizeLSQ(Q, h);
-  ocp.minimizeLSQEndTerm(QN, hN);
-
-  // car can't go backward to avoid "circles"
-  ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
-  // more than absolute max steer angle
-  ocp.subjectTo( deg2rad(-50) <= curvature <= deg2rad(50));
-  ocp.setNOD(2);
-
-  OCPexport mpc(ocp);
-  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
-  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
-  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, 1000);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
-  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
-
-  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
-  mpc.set( QP_SOLVER, QP_QPOASES );
-  mpc.set( HOTSTART_QP, YES );
-  mpc.set( GENERATE_TEST_FILE, NO);
-  mpc.set( GENERATE_MAKE_FILE, NO );
-  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
-  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
-
-  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
-    exit( EXIT_FAILURE );
-
-  mpc.printDimensionsQP( );
-
-  return EXIT_SUCCESS;
-}
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
deleted file mode 100644
index 6765fe57b..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ /dev/null
@@ -1,109 +0,0 @@
-#include "acado_common.h"
-#include "acado_auxiliary_functions.h"
-#include "common/modeldata.h"
-#include <stdio.h>
-
-#define NX          ACADO_NX  /* Number of differential state variables.  */
-#define NXA         ACADO_NXA /* Number of algebraic variables. */
-#define NU          ACADO_NU  /* Number of control inputs. */
-#define NOD         ACADO_NOD  /* Number of online data values. */
-
-#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
-#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
-
-#define N           ACADO_N   /* Number of intervals in the horizon. */
-
-ACADOvariables acadoVariables;
-ACADOworkspace acadoWorkspace;
-
-typedef struct {
-  double x, y, psi, tire_angle, tire_angle_rate;
-} state_t;
-
-typedef struct {
-  double x[N+1];
-  double y[N+1];
-  double psi[N+1];
-  double curvature[N+1];
-  double curvature_rate[N];
-  double cost;
-} log_t;
-
-void set_weights(double pathCost, double headingCost, double steerRateCost){
-  int    i;
-  const int STEP_MULTIPLIER = 3.0;
-
-  for (i = 0; i < N; i++) {
-    double f = 20 * (T_IDXS[i+1] - T_IDXS[i]);
-    // Setup diagonal entries
-    acadoVariables.W[NY*NY*i + (NY+1)*0] = pathCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*1] = headingCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*2] = steerRateCost * f;
-  }
-  acadoVariables.WN[(NYN+1)*0] = pathCost * STEP_MULTIPLIER;
-  acadoVariables.WN[(NYN+1)*1] = headingCost * STEP_MULTIPLIER;
-}
-
-void init(){
-  acado_initializeSolver();
-  int    i;
-
-  /* Initialize the states and controls. */
-  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
-  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
-
-  /* Initialize the measurements/reference. */
-  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
-  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
-
-  /* MPC: initialize the current state feedback. */
-  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
-}
-
-int run_mpc(state_t * x0, log_t * solution, double v_ego,
-             double rotation_radius, double target_y[N+1], double target_psi[N+1]){
-
-  int    i;
-
-  for (i = 0; i <= NOD * N; i+= NOD){
-    acadoVariables.od[i] = v_ego;
-    acadoVariables.od[i+1] = rotation_radius;
-  }
-  for (i = 0; i < N; i+= 1){
-    acadoVariables.y[NY*i + 0] = target_y[i];
-    acadoVariables.y[NY*i + 1] = (v_ego + 5.0) * target_psi[i];
-    acadoVariables.y[NY*i + 2] = 0.0;
-  }
-  acadoVariables.yN[0] = target_y[N];
-  acadoVariables.yN[1] = (2.0 * v_ego + 5.0) * target_psi[N];
-
-  acadoVariables.x0[0] = x0->x;
-  acadoVariables.x0[1] = x0->y;
-  acadoVariables.x0[2] = x0->psi;
-  acadoVariables.x0[3] = x0->tire_angle;
-
-
-  acado_preparationStep();
-  acado_feedbackStep();
-
-  /* printf("lat its: %d\n", acado_getNWSR());  // n iterations
-  printf("Objective: %.6f\n", acado_getObjective());  // solution cost */
-
-  for (i = 0; i <= N; i++){
-    solution->x[i] = acadoVariables.x[i*NX];
-    solution->y[i] = acadoVariables.x[i*NX+1];
-    solution->psi[i] = acadoVariables.x[i*NX+2];
-    solution->curvature[i] = acadoVariables.x[i*NX+3];
-    if (i < N){
-      solution->curvature_rate[i] = acadoVariables.u[i];
-    }
-  }
-  solution->cost = acado_getObjective();
-
-  // Dont shift states here. Current solution is closer to next timestep than if
-  // we use the old solution as a starting point
-  //acado_shiftStates(2, 0, 0);
-  //acado_shiftControls( 0 );
-
-  return acado_getNWSR();
-}
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c
deleted file mode 100644
index 6f0bb705c..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_auxiliary_functions.h"
-
-#include <stdio.h>
-
-real_t* acado_getVariablesX( )
-{
-	return acadoVariables.x;
-}
-
-real_t* acado_getVariablesU( )
-{
-	return acadoVariables.u;
-}
-
-#if ACADO_NY > 0
-real_t* acado_getVariablesY( )
-{
-	return acadoVariables.y;
-}
-#endif
-
-#if ACADO_NYN > 0
-real_t* acado_getVariablesYN( )
-{
-	return acadoVariables.yN;
-}
-#endif
-
-real_t* acado_getVariablesX0( )
-{
-#if ACADO_INITIAL_VALUE_FIXED
-	return acadoVariables.x0;
-#else
-	return 0;
-#endif
-}
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( )
-{
-	int i, j;
-	printf("\nDifferential variables:\n[\n");
-	for (i = 0; i < ACADO_N + 1; ++i)
-	{
-		for (j = 0; j < ACADO_NX; ++j)
-			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print control variables. */
-void acado_printControlVariables( )
-{
-	int i, j;
-	printf("\nControl variables:\n[\n");
-	for (i = 0; i < ACADO_N; ++i)
-	{
-		for (j = 0; j < ACADO_NU; ++j)
-			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( )
-{
-	printf(
-		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
-		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
-		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
-	);
-	
-	printf(
-		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
-		"supervision of Moritz Diehl. All rights reserved.\n\n"
-		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
-		"General Public License 3 in the hope that it will be useful,\n"
-		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
-		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
-		"GNU Lesser General Public License for more details.\n\n"
-	);
-}
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-void acado_tic( acado_timer* t )
-{
-	QueryPerformanceFrequency(&t->freq);
-	QueryPerformanceCounter(&t->tic);
-}
-
-real_t acado_toc( acado_timer* t )
-{
-	QueryPerformanceCounter(&t->toc);
-	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
-}
-
-
-#elif (defined __APPLE__)
-
-void acado_tic( acado_timer* t )
-{
-    /* read current clock cycles */
-    t->tic = mach_absolute_time();
-}
-
-real_t acado_toc( acado_timer* t )
-{
-
-    uint64_t duration; /* elapsed time in clock cycles*/
-    
-    t->toc = mach_absolute_time();
-    duration = t->toc - t->tic;
-    
-    /*conversion from clock cycles to nanoseconds*/
-    mach_timebase_info(&(t->tinfo));
-    duration *= t->tinfo.numer;
-    duration /= t->tinfo.denom;
-
-    return (real_t)duration / 1e9;
-}
-
-#else
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	gettimeofday(&t->tic, 0);
-}
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timeval temp;
-	
-	gettimeofday(&t->toc, 0);
-    
-	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
-}
-
-#else
-/* ANSI */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	clock_gettime(CLOCK_MONOTONIC, &t->tic);
-}
-
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timespec temp;
-    
-	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
-    
-	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
-}
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || _WIN64) */
-
-#endif
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h
deleted file mode 100644
index b1be0a661..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_AUXILIARY_FUNCTIONS_H
-#define ACADO_AUXILIARY_FUNCTIONS_H
-
-#include "acado_common.h"
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** Get pointer to the matrix with differential variables. */
-real_t* acado_getVariablesX( );
-
-/** Get pointer to the matrix with control variables. */
-real_t* acado_getVariablesU( );
-
-#if ACADO_NY > 0
-/** Get pointer to the matrix with references/measurements. */
-real_t* acado_getVariablesY( );
-#endif
-
-#if ACADO_NYN > 0
-/** Get pointer to the vector with references/measurement on the last node. */
-real_t* acado_getVariablesYN( );
-#endif
-
-/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
-real_t* acado_getVariablesX0( );
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( );
-
-/** Print control variables. */
-void acado_printControlVariables( );
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( );
-
-/*
- * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
- * providing us with the following timing routines.
- */
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-/* Use Windows QueryPerformanceCounter for timing. */
-#include <Windows.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	LARGE_INTEGER tic;
-	LARGE_INTEGER toc;
-	LARGE_INTEGER freq;
-} acado_timer;
-
-
-#elif (defined __APPLE__)
-
-#include "unistd.h"
-#include <mach/mach_time.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	uint64_t tic;
-	uint64_t toc;
-	mach_timebase_info_data_t tinfo;
-} acado_timer;
-
-#else
-
-/* Use POSIX clock_gettime() for timing on non-Windows machines. */
-#include <time.h>
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode of operation. */
-
-#include <sys/stat.h>
-#include <sys/time.h>
-
-typedef struct acado_timer_
-{
-	struct timeval tic;
-	struct timeval toc;
-} acado_timer;
-
-#else
-/* ANSI C */
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	struct timespec tic;
-	struct timespec toc;
-} acado_timer;
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || defined _WIN64) */
-
-/** A function for measurement of the current time. */
-void acado_tic( acado_timer* t );
-
-/** A function which returns the elapsed time. */
-real_t acado_toc( acado_timer* t );
-
-#endif
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_AUXILIARY_FUNCTIONS_H */
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
deleted file mode 100644
index 4e48894de..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_common.h
+++ /dev/null
@@ -1,354 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_COMMON_H
-#define ACADO_COMMON_H
-
-#include <math.h>
-#include <string.h>
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** \defgroup ACADO ACADO CGT generated module. */
-/** @{ */
-
-/** qpOASES QP solver indicator. */
-#define ACADO_QPOASES  0
-#define ACADO_QPOASES3 1
-/** FORCES QP solver indicator.*/
-#define ACADO_FORCES   2
-/** qpDUNES QP solver indicator.*/
-#define ACADO_QPDUNES  3
-/** HPMPC QP solver indicator. */
-#define ACADO_HPMPC    4
-#define ACADO_GENERIC    5
-
-/** Indicator for determining the QP solver used by the ACADO solver code. */
-#define ACADO_QP_SOLVER ACADO_QPOASES
-
-#include "acado_qpoases_interface.hpp"
-
-
-/*
- * Common definitions
- */
-/** User defined block based condensing. */
-#define ACADO_BLOCK_CONDENSING 0
-/** Compute covariance matrix of the last state estimate. */
-#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
-/** Flag indicating whether constraint values are hard-coded or not. */
-#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
-/** Indicator for fixed initial state. */
-#define ACADO_INITIAL_STATE_FIXED 1
-/** Number of control/estimation intervals. */
-#define ACADO_N 16
-/** Number of online data values. */
-#define ACADO_NOD 2
-/** Number of path constraints. */
-#define ACADO_NPAC 0
-/** Number of control variables. */
-#define ACADO_NU 1
-/** Number of differential variables. */
-#define ACADO_NX 4
-/** Number of algebraic variables. */
-#define ACADO_NXA 0
-/** Number of differential derivative variables. */
-#define ACADO_NXD 0
-/** Number of references/measurements per node on the first N nodes. */
-#define ACADO_NY 3
-/** Number of references/measurements on the last (N + 1)st node. */
-#define ACADO_NYN 2
-/** Total number of QP optimization variables. */
-#define ACADO_QP_NV 20
-/** Number of Runge-Kutta stages per integration step. */
-#define ACADO_RK_NSTAGES 4
-/** Providing interface for arrival cost. */
-#define ACADO_USE_ARRIVAL_COST 0
-/** Indicator for usage of non-hard-coded linear terms in the objective. */
-#define ACADO_USE_LINEAR_TERMS 0
-/** Indicator for type of fixed weighting matrices. */
-#define ACADO_WEIGHTING_MATRICES_TYPE 2
-
-
-/*
- * Globally used structure definitions
- */
-
-/** The structure containing the user data.
- * 
- *  Via this structure the user "communicates" with the solver code.
- */
-typedef struct ACADOvariables_
-{
-int dummy;
-/** Matrix of size: 17 x 4 (row major format)
- * 
- *  Matrix containing 17 differential variable vectors.
- */
-real_t x[ 68 ];
-
-/** Column vector of size: 16
- * 
- *  Matrix containing 16 control variable vectors.
- */
-real_t u[ 16 ];
-
-/** Matrix of size: 17 x 2 (row major format)
- * 
- *  Matrix containing 17 online data vectors.
- */
-real_t od[ 34 ];
-
-/** Column vector of size: 48
- * 
- *  Matrix containing 16 reference/measurement vectors of size 3 for first 16 nodes.
- */
-real_t y[ 48 ];
-
-/** Column vector of size: 2
- * 
- *  Reference/measurement vector for the 17. node.
- */
-real_t yN[ 2 ];
-
-/** Matrix of size: 48 x 3 (row major format) */
-real_t W[ 144 ];
-
-/** Matrix of size: 2 x 2 (row major format) */
-real_t WN[ 4 ];
-
-/** Column vector of size: 4
- * 
- *  Current state feedback vector.
- */
-real_t x0[ 4 ];
-
-
-} ACADOvariables;
-
-/** Private workspace used by the auto-generated code.
- * 
- *  Data members of this structure are private to the solver.
- *  In other words, the user code should not modify values of this 
- *  structure. 
- */
-typedef struct ACADOworkspace_
-{
-/** Column vector of size: 14 */
-real_t rhs_aux[ 14 ];
-
-real_t rk_ttt;
-
-/** Row vector of size: 27 */
-real_t rk_xxx[ 27 ];
-
-/** Matrix of size: 4 x 24 (row major format) */
-real_t rk_kkk[ 96 ];
-
-/** Row vector of size: 27 */
-real_t state[ 27 ];
-
-/** Column vector of size: 64 */
-real_t d[ 64 ];
-
-/** Column vector of size: 48 */
-real_t Dy[ 48 ];
-
-/** Column vector of size: 2 */
-real_t DyN[ 2 ];
-
-/** Matrix of size: 64 x 4 (row major format) */
-real_t evGx[ 256 ];
-
-/** Column vector of size: 64 */
-real_t evGu[ 64 ];
-
-/** Row vector of size: 7 */
-real_t objValueIn[ 7 ];
-
-/** Row vector of size: 18 */
-real_t objValueOut[ 18 ];
-
-/** Matrix of size: 64 x 4 (row major format) */
-real_t Q1[ 256 ];
-
-/** Matrix of size: 64 x 3 (row major format) */
-real_t Q2[ 192 ];
-
-/** Column vector of size: 16 */
-real_t R1[ 16 ];
-
-/** Matrix of size: 16 x 3 (row major format) */
-real_t R2[ 48 ];
-
-/** Column vector of size: 64 */
-real_t S1[ 64 ];
-
-/** Matrix of size: 4 x 4 (row major format) */
-real_t QN1[ 16 ];
-
-/** Matrix of size: 4 x 2 (row major format) */
-real_t QN2[ 8 ];
-
-/** Column vector of size: 4 */
-real_t Dx0[ 4 ];
-
-/** Matrix of size: 4 x 4 (row major format) */
-real_t T[ 16 ];
-
-/** Column vector of size: 544 */
-real_t E[ 544 ];
-
-/** Column vector of size: 544 */
-real_t QE[ 544 ];
-
-/** Matrix of size: 64 x 4 (row major format) */
-real_t QGx[ 256 ];
-
-/** Column vector of size: 64 */
-real_t Qd[ 64 ];
-
-/** Column vector of size: 68 */
-real_t QDy[ 68 ];
-
-/** Matrix of size: 16 x 4 (row major format) */
-real_t H10[ 64 ];
-
-/** Matrix of size: 20 x 20 (row major format) */
-real_t H[ 400 ];
-
-/** Matrix of size: 32 x 20 (row major format) */
-real_t A[ 640 ];
-
-/** Column vector of size: 20 */
-real_t g[ 20 ];
-
-/** Column vector of size: 20 */
-real_t lb[ 20 ];
-
-/** Column vector of size: 20 */
-real_t ub[ 20 ];
-
-/** Column vector of size: 32 */
-real_t lbA[ 32 ];
-
-/** Column vector of size: 32 */
-real_t ubA[ 32 ];
-
-/** Column vector of size: 20 */
-real_t x[ 20 ];
-
-/** Column vector of size: 52 */
-real_t y[ 52 ];
-
-
-} ACADOworkspace;
-
-/* 
- * Forward function declarations. 
- */
-
-
-/** Performs the integration and sensitivity propagation for one shooting interval.
- *
- *  \param rk_eta Working array to pass the input values and return the results.
- *  \param resetIntegrator The internal memory of the integrator can be reset.
- *  \param rk_index Number of the shooting interval.
- *
- *  \return Status code of the integrator.
- */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
-
-/** Export of an ACADO symbolic function.
- *
- *  \param in Input to the exported function.
- *  \param out Output of the exported function.
- */
-void acado_rhs_forw(const real_t* in, real_t* out);
-
-/** Preparation step of the RTI scheme.
- *
- *  \return Status of the integration module. =0: OK, otherwise the error code.
- */
-int acado_preparationStep(  );
-
-/** Feedback/estimation step of the RTI scheme.
- *
- *  \return Status code of the qpOASES QP solver.
- */
-int acado_feedbackStep(  );
-
-/** Solver initialization. Must be called once before any other function call.
- *
- *  \return =0: OK, otherwise an error code of a QP solver.
- */
-int acado_initializeSolver(  );
-
-/** Initialize shooting nodes by a forward simulation starting from the first node.
- */
-void acado_initializeNodesByForwardSimulation(  );
-
-/** Shift differential variables vector by one interval.
- *
- *  \param strategy Shifting strategy: 1. Initialize node 17 with xEnd. 2. Initialize node 17 by forward simulation.
- *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
-
-/** Shift controls vector by one interval.
- *
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftControls( real_t* const uEnd );
-
-/** Get the KKT tolerance of the current iterate.
- *
- *  \return The KKT tolerance value.
- */
-real_t acado_getKKT(  );
-
-/** Calculate the objective value.
- *
- *  \return Value of the objective function.
- */
-real_t acado_getObjective(  );
-
-
-/* 
- * Extern declarations. 
- */
-
-extern ACADOworkspace acadoWorkspace;
-extern ACADOvariables acadoVariables;
-
-/** @} */
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_COMMON_H */
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
deleted file mode 100644
index df3b7133e..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.c
+++ /dev/null
@@ -1,240 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-void acado_rhs_forw(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 24;
-const real_t* od = in + 25;
-/* Vector of auxiliary variables; number of elements: 14. */
-real_t* a = acadoWorkspace.rhs_aux;
-
-/* Compute intermediate quantities: */
-a[0] = (cos(xd[2]));
-a[1] = (sin(xd[2]));
-a[2] = ((real_t)(-1.0000000000000000e+00)*(sin(xd[2])));
-a[3] = (xd[12]*a[2]);
-a[4] = (xd[13]*a[2]);
-a[5] = (xd[14]*a[2]);
-a[6] = (xd[15]*a[2]);
-a[7] = (cos(xd[2]));
-a[8] = (xd[12]*a[7]);
-a[9] = (xd[13]*a[7]);
-a[10] = (xd[14]*a[7]);
-a[11] = (xd[15]*a[7]);
-a[12] = (xd[22]*a[2]);
-a[13] = (xd[22]*a[7]);
-
-/* Compute outputs: */
-out[0] = (od[0]*a[0]);
-out[1] = (od[0]*a[1]);
-out[2] = (od[0]*xd[3]);
-out[3] = u[0];
-out[4] = (od[0]*a[3]);
-out[5] = (od[0]*a[4]);
-out[6] = (od[0]*a[5]);
-out[7] = (od[0]*a[6]);
-out[8] = (od[0]*a[8]);
-out[9] = (od[0]*a[9]);
-out[10] = (od[0]*a[10]);
-out[11] = (od[0]*a[11]);
-out[12] = (od[0]*xd[16]);
-out[13] = (od[0]*xd[17]);
-out[14] = (od[0]*xd[18]);
-out[15] = (od[0]*xd[19]);
-out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = (real_t)(0.0000000000000000e+00);
-out[18] = (real_t)(0.0000000000000000e+00);
-out[19] = (real_t)(0.0000000000000000e+00);
-out[20] = (od[0]*a[12]);
-out[21] = (od[0]*a[13]);
-out[22] = (od[0]*xd[23]);
-out[23] = (real_t)(1.0000000000000000e+00);
-}
-
-/* Fixed step size:0.0025 */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
-{
-int error;
-
-int run1;
-int numSteps[16] = {4, 12, 20, 27, 35, 43, 51, 59, 66, 74, 82, 90, 98, 105, 113, 121};
-int numInts = numSteps[rk_index];
-acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
-rk_eta[4] = 1.0000000000000000e+00;
-rk_eta[5] = 0.0000000000000000e+00;
-rk_eta[6] = 0.0000000000000000e+00;
-rk_eta[7] = 0.0000000000000000e+00;
-rk_eta[8] = 0.0000000000000000e+00;
-rk_eta[9] = 1.0000000000000000e+00;
-rk_eta[10] = 0.0000000000000000e+00;
-rk_eta[11] = 0.0000000000000000e+00;
-rk_eta[12] = 0.0000000000000000e+00;
-rk_eta[13] = 0.0000000000000000e+00;
-rk_eta[14] = 1.0000000000000000e+00;
-rk_eta[15] = 0.0000000000000000e+00;
-rk_eta[16] = 0.0000000000000000e+00;
-rk_eta[17] = 0.0000000000000000e+00;
-rk_eta[18] = 0.0000000000000000e+00;
-rk_eta[19] = 1.0000000000000000e+00;
-rk_eta[20] = 0.0000000000000000e+00;
-rk_eta[21] = 0.0000000000000000e+00;
-rk_eta[22] = 0.0000000000000000e+00;
-rk_eta[23] = 0.0000000000000000e+00;
-acadoWorkspace.rk_xxx[24] = rk_eta[24];
-acadoWorkspace.rk_xxx[25] = rk_eta[25];
-acadoWorkspace.rk_xxx[26] = rk_eta[26];
-
-for (run1 = 0; run1 < 1; ++run1)
-{
-for(run1 = 0; run1 < numInts; run1++ ) {
-acadoWorkspace.rk_xxx[0] = + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + rk_eta[23];
-acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 24 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[24] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[25] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[26] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[27] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[28] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[29] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[30] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[31] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[32] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[33] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[34] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[35] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[36] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[37] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[38] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[39] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[40] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[41] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[42] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[43] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[44] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[45] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[46] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)1.2500000000000000e-03*acadoWorkspace.rk_kkk[47] + rk_eta[23];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[48] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[49] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[50] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[51] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[52] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[53] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[54] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[55] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[56] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[57] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[58] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[59] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[60] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[61] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[62] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[63] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[64] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[65] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[66] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[67] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[68] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[69] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[70] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)2.5000000000000001e-03*acadoWorkspace.rk_kkk[71] + rk_eta[23];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 72 ]) );
-rk_eta[0] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[0] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[24] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[48] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[72];
-rk_eta[1] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[1] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[25] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[49] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[73];
-rk_eta[2] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[2] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[26] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[50] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[74];
-rk_eta[3] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[3] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[27] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[51] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[75];
-rk_eta[4] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[4] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[28] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[52] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[76];
-rk_eta[5] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[5] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[29] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[53] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[77];
-rk_eta[6] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[6] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[30] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[54] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[78];
-rk_eta[7] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[7] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[31] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[55] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[79];
-rk_eta[8] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[8] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[32] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[56] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[80];
-rk_eta[9] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[9] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[33] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[57] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[81];
-rk_eta[10] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[10] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[34] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[58] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[82];
-rk_eta[11] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[11] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[35] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[59] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[83];
-rk_eta[12] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[12] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[36] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[60] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[84];
-rk_eta[13] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[13] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[37] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[61] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[85];
-rk_eta[14] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[14] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[38] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[62] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[86];
-rk_eta[15] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[15] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[39] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[63] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[87];
-rk_eta[16] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[16] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[40] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[64] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[88];
-rk_eta[17] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[17] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[41] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[65] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[89];
-rk_eta[18] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[18] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[42] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[66] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[90];
-rk_eta[19] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[19] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[43] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[67] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[91];
-rk_eta[20] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[20] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[44] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[68] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[92];
-rk_eta[21] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[21] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[45] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[69] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[93];
-rk_eta[22] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[22] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[46] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[70] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[94];
-rk_eta[23] += + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[23] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[47] + (real_t)8.3333333333333328e-04*acadoWorkspace.rk_kkk[71] + (real_t)4.1666666666666664e-04*acadoWorkspace.rk_kkk[95];
-acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
-}
-}
-error = 0;
-return error;
-}
-
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp
deleted file mode 100644
index 86b5f1548..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-extern "C"
-{
-#include "acado_common.h"
-}
-
-#include "INCLUDE/QProblem.hpp"
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-static int acado_nWSR;
-
-
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-static SolutionAnalysis acado_sa;
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-int acado_solve( void )
-{
-	acado_nWSR = QPOASES_NWSRMAX;
-
-	QProblem qp(20, 32);
-	
-	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
-
-    qp.getPrimalSolution( acadoWorkspace.x );
-    qp.getDualSolution( acadoWorkspace.y );
-	
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-
-	if (retVal != SUCCESSFUL_RETURN)
-		return (int)retVal;
-		
-	retVal = acado_sa.getHessianInverse( &qp,var );
-
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-	return (int)retVal;
-}
-
-int acado_getNWSR( void )
-{
-	return acado_nWSR;
-}
-
-const char* acado_getErrorString( int error )
-{
-	return MessageHandling::getErrorString( error );
-}
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp
deleted file mode 100644
index 723e484d6..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef QPOASES_HEADER
-#define QPOASES_HEADER
-
-#ifdef PC_DEBUG
-#include <stdio.h>
-#endif /* PC_DEBUG */
-
-#include <math.h>
-
-#ifdef __cplusplus
-#define EXTERNC extern "C"
-#else
-#define EXTERNC
-#endif
-
-/*
- * A set of options for qpOASES
- */
-
-/** Maximum number of optimization variables. */
-#define QPOASES_NVMAX      20
-/** Maximum number of constraints. */
-#define QPOASES_NCMAX      32
-/** Maximum number of working set recalculations. */
-#define QPOASES_NWSRMAX    50
-/** Print level for qpOASES. */
-#define QPOASES_PRINTLEVEL PL_NONE
-/** The value of EPS */
-#define QPOASES_EPS        2.221e-16
-/** Internally used floating point type */
-typedef double real_t;
-
-/*
- * Forward function declarations
- */
-
-/** A function that calls the QP solver */
-EXTERNC int acado_solve( void );
-
-/** Get the number of active set changes */
-EXTERNC int acado_getNWSR( void );
-
-/** Get the error string. */
-const char* acado_getErrorString( int error );
-
-#endif /* QPOASES_HEADER */
diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
deleted file mode 100644
index c8c7d27d6..000000000
--- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.c
+++ /dev/null
@@ -1,3384 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-
-
-/******************************************************************************/
-/*                                                                            */
-/* ACADO code generation                                                      */
-/*                                                                            */
-/******************************************************************************/
-
-
-int acado_modelSimulation(  )
-{
-int ret;
-
-int lRun1;
-ret = 0;
-for (lRun1 = 0; lRun1 < 16; ++lRun1)
-{
-acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4];
-acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3];
-
-acadoWorkspace.state[24] = acadoVariables.u[lRun1];
-acadoWorkspace.state[25] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.state[26] = acadoVariables.od[lRun1 * 2 + 1];
-
-ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
-
-acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4];
-acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5];
-acadoWorkspace.d[lRun1 * 4 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 4 + 6];
-acadoWorkspace.d[lRun1 * 4 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 4 + 7];
-
-acadoWorkspace.evGx[lRun1 * 16] = acadoWorkspace.state[4];
-acadoWorkspace.evGx[lRun1 * 16 + 1] = acadoWorkspace.state[5];
-acadoWorkspace.evGx[lRun1 * 16 + 2] = acadoWorkspace.state[6];
-acadoWorkspace.evGx[lRun1 * 16 + 3] = acadoWorkspace.state[7];
-acadoWorkspace.evGx[lRun1 * 16 + 4] = acadoWorkspace.state[8];
-acadoWorkspace.evGx[lRun1 * 16 + 5] = acadoWorkspace.state[9];
-acadoWorkspace.evGx[lRun1 * 16 + 6] = acadoWorkspace.state[10];
-acadoWorkspace.evGx[lRun1 * 16 + 7] = acadoWorkspace.state[11];
-acadoWorkspace.evGx[lRun1 * 16 + 8] = acadoWorkspace.state[12];
-acadoWorkspace.evGx[lRun1 * 16 + 9] = acadoWorkspace.state[13];
-acadoWorkspace.evGx[lRun1 * 16 + 10] = acadoWorkspace.state[14];
-acadoWorkspace.evGx[lRun1 * 16 + 11] = acadoWorkspace.state[15];
-acadoWorkspace.evGx[lRun1 * 16 + 12] = acadoWorkspace.state[16];
-acadoWorkspace.evGx[lRun1 * 16 + 13] = acadoWorkspace.state[17];
-acadoWorkspace.evGx[lRun1 * 16 + 14] = acadoWorkspace.state[18];
-acadoWorkspace.evGx[lRun1 * 16 + 15] = acadoWorkspace.state[19];
-
-acadoWorkspace.evGu[lRun1 * 4] = acadoWorkspace.state[20];
-acadoWorkspace.evGu[lRun1 * 4 + 1] = acadoWorkspace.state[21];
-acadoWorkspace.evGu[lRun1 * 4 + 2] = acadoWorkspace.state[22];
-acadoWorkspace.evGu[lRun1 * 4 + 3] = acadoWorkspace.state[23];
-}
-return ret;
-}
-
-void acado_evaluateLSQ(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 4;
-const real_t* od = in + 5;
-
-/* Compute outputs: */
-out[0] = xd[1];
-out[1] = ((od[0]+(real_t)(5.0000000000000000e+00))*xd[2]);
-out[2] = (((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00))*u[0]);
-out[3] = (real_t)(0.0000000000000000e+00);
-out[4] = (real_t)(1.0000000000000000e+00);
-out[5] = (real_t)(0.0000000000000000e+00);
-out[6] = (real_t)(0.0000000000000000e+00);
-out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (od[0]+(real_t)(5.0000000000000000e+00));
-out[10] = (real_t)(0.0000000000000000e+00);
-out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = (real_t)(0.0000000000000000e+00);
-out[14] = (real_t)(0.0000000000000000e+00);
-out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = ((od[0]+(real_t)(5.0000000000000000e+00))*(real_t)(4.0000000000000000e+00));
-}
-
-void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* od = in + 4;
-
-/* Compute outputs: */
-out[0] = xd[1];
-out[1] = ((((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00))*xd[2]);
-out[2] = (real_t)(0.0000000000000000e+00);
-out[3] = (real_t)(1.0000000000000000e+00);
-out[4] = (real_t)(0.0000000000000000e+00);
-out[5] = (real_t)(0.0000000000000000e+00);
-out[6] = (real_t)(0.0000000000000000e+00);
-out[7] = (real_t)(0.0000000000000000e+00);
-out[8] = (((real_t)(2.0000000000000000e+00)*od[0])+(real_t)(5.0000000000000000e+00));
-out[9] = (real_t)(0.0000000000000000e+00);
-}
-
-void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 )
-{
-tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[4]*tmpObjS[3] + tmpFx[8]*tmpObjS[6];
-tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[4]*tmpObjS[4] + tmpFx[8]*tmpObjS[7];
-tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[4]*tmpObjS[5] + tmpFx[8]*tmpObjS[8];
-tmpQ2[3] = + tmpFx[1]*tmpObjS[0] + tmpFx[5]*tmpObjS[3] + tmpFx[9]*tmpObjS[6];
-tmpQ2[4] = + tmpFx[1]*tmpObjS[1] + tmpFx[5]*tmpObjS[4] + tmpFx[9]*tmpObjS[7];
-tmpQ2[5] = + tmpFx[1]*tmpObjS[2] + tmpFx[5]*tmpObjS[5] + tmpFx[9]*tmpObjS[8];
-tmpQ2[6] = + tmpFx[2]*tmpObjS[0] + tmpFx[6]*tmpObjS[3] + tmpFx[10]*tmpObjS[6];
-tmpQ2[7] = + tmpFx[2]*tmpObjS[1] + tmpFx[6]*tmpObjS[4] + tmpFx[10]*tmpObjS[7];
-tmpQ2[8] = + tmpFx[2]*tmpObjS[2] + tmpFx[6]*tmpObjS[5] + tmpFx[10]*tmpObjS[8];
-tmpQ2[9] = + tmpFx[3]*tmpObjS[0] + tmpFx[7]*tmpObjS[3] + tmpFx[11]*tmpObjS[6];
-tmpQ2[10] = + tmpFx[3]*tmpObjS[1] + tmpFx[7]*tmpObjS[4] + tmpFx[11]*tmpObjS[7];
-tmpQ2[11] = + tmpFx[3]*tmpObjS[2] + tmpFx[7]*tmpObjS[5] + tmpFx[11]*tmpObjS[8];
-tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[8];
-tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[9];
-tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[10];
-tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[11];
-tmpQ1[4] = + tmpQ2[3]*tmpFx[0] + tmpQ2[4]*tmpFx[4] + tmpQ2[5]*tmpFx[8];
-tmpQ1[5] = + tmpQ2[3]*tmpFx[1] + tmpQ2[4]*tmpFx[5] + tmpQ2[5]*tmpFx[9];
-tmpQ1[6] = + tmpQ2[3]*tmpFx[2] + tmpQ2[4]*tmpFx[6] + tmpQ2[5]*tmpFx[10];
-tmpQ1[7] = + tmpQ2[3]*tmpFx[3] + tmpQ2[4]*tmpFx[7] + tmpQ2[5]*tmpFx[11];
-tmpQ1[8] = + tmpQ2[6]*tmpFx[0] + tmpQ2[7]*tmpFx[4] + tmpQ2[8]*tmpFx[8];
-tmpQ1[9] = + tmpQ2[6]*tmpFx[1] + tmpQ2[7]*tmpFx[5] + tmpQ2[8]*tmpFx[9];
-tmpQ1[10] = + tmpQ2[6]*tmpFx[2] + tmpQ2[7]*tmpFx[6] + tmpQ2[8]*tmpFx[10];
-tmpQ1[11] = + tmpQ2[6]*tmpFx[3] + tmpQ2[7]*tmpFx[7] + tmpQ2[8]*tmpFx[11];
-tmpQ1[12] = + tmpQ2[9]*tmpFx[0] + tmpQ2[10]*tmpFx[4] + tmpQ2[11]*tmpFx[8];
-tmpQ1[13] = + tmpQ2[9]*tmpFx[1] + tmpQ2[10]*tmpFx[5] + tmpQ2[11]*tmpFx[9];
-tmpQ1[14] = + tmpQ2[9]*tmpFx[2] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[10];
-tmpQ1[15] = + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[11];
-}
-
-void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 )
-{
-tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[3] + tmpFu[2]*tmpObjS[6];
-tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[4] + tmpFu[2]*tmpObjS[7];
-tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[8];
-tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2];
-}
-
-void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 )
-{
-tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[2];
-tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[3];
-tmpQN2[2] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[2];
-tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[3];
-tmpQN2[4] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[2];
-tmpQN2[5] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[3];
-tmpQN2[6] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[2];
-tmpQN2[7] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[3];
-tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[4];
-tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[5];
-tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[6];
-tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[7];
-tmpQN1[4] = + tmpQN2[2]*tmpFx[0] + tmpQN2[3]*tmpFx[4];
-tmpQN1[5] = + tmpQN2[2]*tmpFx[1] + tmpQN2[3]*tmpFx[5];
-tmpQN1[6] = + tmpQN2[2]*tmpFx[2] + tmpQN2[3]*tmpFx[6];
-tmpQN1[7] = + tmpQN2[2]*tmpFx[3] + tmpQN2[3]*tmpFx[7];
-tmpQN1[8] = + tmpQN2[4]*tmpFx[0] + tmpQN2[5]*tmpFx[4];
-tmpQN1[9] = + tmpQN2[4]*tmpFx[1] + tmpQN2[5]*tmpFx[5];
-tmpQN1[10] = + tmpQN2[4]*tmpFx[2] + tmpQN2[5]*tmpFx[6];
-tmpQN1[11] = + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[7];
-tmpQN1[12] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[4];
-tmpQN1[13] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[5];
-tmpQN1[14] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[6];
-tmpQN1[15] = + tmpQN2[6]*tmpFx[3] + tmpQN2[7]*tmpFx[7];
-}
-
-void acado_evaluateObjective(  )
-{
-int runObj;
-for (runObj = 0; runObj < 16; ++runObj)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[runObj * 3] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.Dy[runObj * 3 + 1] = acadoWorkspace.objValueOut[1];
-acadoWorkspace.Dy[runObj * 3 + 2] = acadoWorkspace.objValueOut[2];
-
-acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 3 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) );
-
-acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 15 ]), &(acadoVariables.W[ runObj * 9 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 3 ]) );
-
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[64];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[65];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[66];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[67];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[32];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[33];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1];
-
-acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 2 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 );
-
-}
-
-void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew )
-{
-dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3];
-dNew[1] += + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3];
-dNew[2] += + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3];
-dNew[3] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3];
-}
-
-void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 )
-{
-Gx2[0] = Gx1[0];
-Gx2[1] = Gx1[1];
-Gx2[2] = Gx1[2];
-Gx2[3] = Gx1[3];
-Gx2[4] = Gx1[4];
-Gx2[5] = Gx1[5];
-Gx2[6] = Gx1[6];
-Gx2[7] = Gx1[7];
-Gx2[8] = Gx1[8];
-Gx2[9] = Gx1[9];
-Gx2[10] = Gx1[10];
-Gx2[11] = Gx1[11];
-Gx2[12] = Gx1[12];
-Gx2[13] = Gx1[13];
-Gx2[14] = Gx1[14];
-Gx2[15] = Gx1[15];
-}
-
-void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 )
-{
-Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[8] + Gx1[3]*Gx2[12];
-Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[9] + Gx1[3]*Gx2[13];
-Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[10] + Gx1[3]*Gx2[14];
-Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[11] + Gx1[3]*Gx2[15];
-Gx3[4] = + Gx1[4]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[6]*Gx2[8] + Gx1[7]*Gx2[12];
-Gx3[5] = + Gx1[4]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[6]*Gx2[9] + Gx1[7]*Gx2[13];
-Gx3[6] = + Gx1[4]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[6]*Gx2[10] + Gx1[7]*Gx2[14];
-Gx3[7] = + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[6]*Gx2[11] + Gx1[7]*Gx2[15];
-Gx3[8] = + Gx1[8]*Gx2[0] + Gx1[9]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[11]*Gx2[12];
-Gx3[9] = + Gx1[8]*Gx2[1] + Gx1[9]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[11]*Gx2[13];
-Gx3[10] = + Gx1[8]*Gx2[2] + Gx1[9]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[11]*Gx2[14];
-Gx3[11] = + Gx1[8]*Gx2[3] + Gx1[9]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[11]*Gx2[15];
-Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[4] + Gx1[14]*Gx2[8] + Gx1[15]*Gx2[12];
-Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[5] + Gx1[14]*Gx2[9] + Gx1[15]*Gx2[13];
-Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[10] + Gx1[15]*Gx2[14];
-Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[11] + Gx1[15]*Gx2[15];
-}
-
-void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2] + Gx1[3]*Gu1[3];
-Gu2[1] = + Gx1[4]*Gu1[0] + Gx1[5]*Gu1[1] + Gx1[6]*Gu1[2] + Gx1[7]*Gu1[3];
-Gu2[2] = + Gx1[8]*Gu1[0] + Gx1[9]*Gu1[1] + Gx1[10]*Gu1[2] + Gx1[11]*Gu1[3];
-Gu2[3] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[1] + Gx1[14]*Gu1[2] + Gx1[15]*Gu1[3];
-}
-
-void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = Gu1[0];
-Gu2[1] = Gu1[1];
-Gu2[2] = Gu1[2];
-Gu2[3] = Gu1[3];
-}
-
-void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 )
-{
-acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3];
-}
-
-void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 )
-{
-acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = R11[0];
-}
-
-void acado_zeroBlockH11( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = 0.0000000000000000e+00;
-}
-
-void acado_copyHTH( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 20 + 80) + (iCol + 4)] = acadoWorkspace.H[(iCol * 20 + 80) + (iRow + 4)];
-}
-
-void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3];
-dNew[1] = + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3];
-dNew[2] = + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3];
-dNew[3] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3];
-}
-
-void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3];
-dNew[1] = + acadoWorkspace.QN1[4]*dOld[0] + acadoWorkspace.QN1[5]*dOld[1] + acadoWorkspace.QN1[6]*dOld[2] + acadoWorkspace.QN1[7]*dOld[3];
-dNew[2] = + acadoWorkspace.QN1[8]*dOld[0] + acadoWorkspace.QN1[9]*dOld[1] + acadoWorkspace.QN1[10]*dOld[2] + acadoWorkspace.QN1[11]*dOld[3];
-dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3];
-}
-
-void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 )
-{
-RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2];
-}
-
-void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 )
-{
-QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2];
-QDy1[1] = + Q2[3]*Dy1[0] + Q2[4]*Dy1[1] + Q2[5]*Dy1[2];
-QDy1[2] = + Q2[6]*Dy1[0] + Q2[7]*Dy1[1] + Q2[8]*Dy1[2];
-QDy1[3] = + Q2[9]*Dy1[0] + Q2[10]*Dy1[1] + Q2[11]*Dy1[2];
-}
-
-void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 )
-{
-U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2] + E1[3]*QDy1[3];
-}
-
-void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 )
-{
-H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[4] + E1[2]*Gx1[8] + E1[3]*Gx1[12];
-H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[5] + E1[2]*Gx1[9] + E1[3]*Gx1[13];
-H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[6] + E1[2]*Gx1[10] + E1[3]*Gx1[14];
-H101[3] += + E1[0]*Gx1[3] + E1[1]*Gx1[7] + E1[2]*Gx1[11] + E1[3]*Gx1[15];
-}
-
-void acado_zeroBlockH10( real_t* const H101 )
-{
-{ int lCopy; for (lCopy = 0; lCopy < 4; lCopy++) H101[ lCopy ] = 0; }
-}
-
-void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew )
-{
-dNew[0] += + E1[0]*U1[0];
-dNew[1] += + E1[1]*U1[0];
-dNew[2] += + E1[2]*U1[0];
-dNew[3] += + E1[3]*U1[0];
-}
-
-void acado_zeroBlockH00(  )
-{
-acadoWorkspace.H[0] = 0.0000000000000000e+00;
-acadoWorkspace.H[1] = 0.0000000000000000e+00;
-acadoWorkspace.H[2] = 0.0000000000000000e+00;
-acadoWorkspace.H[3] = 0.0000000000000000e+00;
-acadoWorkspace.H[20] = 0.0000000000000000e+00;
-acadoWorkspace.H[21] = 0.0000000000000000e+00;
-acadoWorkspace.H[22] = 0.0000000000000000e+00;
-acadoWorkspace.H[23] = 0.0000000000000000e+00;
-acadoWorkspace.H[40] = 0.0000000000000000e+00;
-acadoWorkspace.H[41] = 0.0000000000000000e+00;
-acadoWorkspace.H[42] = 0.0000000000000000e+00;
-acadoWorkspace.H[43] = 0.0000000000000000e+00;
-acadoWorkspace.H[60] = 0.0000000000000000e+00;
-acadoWorkspace.H[61] = 0.0000000000000000e+00;
-acadoWorkspace.H[62] = 0.0000000000000000e+00;
-acadoWorkspace.H[63] = 0.0000000000000000e+00;
-}
-
-void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 )
-{
-acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12]*Gx2[12];
-acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13];
-acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14];
-acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15];
-acadoWorkspace.H[20] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12];
-acadoWorkspace.H[21] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13];
-acadoWorkspace.H[22] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14];
-acadoWorkspace.H[23] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15];
-acadoWorkspace.H[40] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12];
-acadoWorkspace.H[41] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13];
-acadoWorkspace.H[42] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14];
-acadoWorkspace.H[43] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15];
-acadoWorkspace.H[60] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12];
-acadoWorkspace.H[61] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13];
-acadoWorkspace.H[62] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14];
-acadoWorkspace.H[63] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15];
-}
-
-void acado_macCTSlx( real_t* const C0, real_t* const g0 )
-{
-g0[0] += 0.0;
-;
-g0[1] += 0.0;
-;
-g0[2] += 0.0;
-;
-g0[3] += 0.0;
-;
-}
-
-void acado_macETSlu( real_t* const E0, real_t* const g1 )
-{
-g1[0] += 0.0;
-;
-}
-
-void acado_condensePrep(  )
-{
-int lRun1;
-int lRun2;
-int lRun3;
-int lRun4;
-int lRun5;
-/** Row vector of size: 32 */
-static const int xBoundIndices[ 32 ] = 
-{ 6, 7, 10, 11, 14, 15, 18, 19, 22, 23, 26, 27, 30, 31, 34, 35, 38, 39, 42, 43, 46, 47, 50, 51, 54, 55, 58, 59, 62, 63, 66, 67 };
-acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E );
-acado_moveGxT( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.T );
-acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.d[ 4 ]) );
-acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 16 ]) );
-
-acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 4 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 4 ]), &(acadoWorkspace.E[ 8 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.d[ 8 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.evGx[ 32 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.E[ 12 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.E[ 16 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 8 ]), &(acadoWorkspace.E[ 20 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.d[ 12 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.evGx[ 48 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.E[ 28 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.E[ 32 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 36 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.d[ 16 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.evGx[ 64 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 40 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.E[ 44 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.E[ 48 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 52 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 16 ]), &(acadoWorkspace.E[ 56 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.d[ 20 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.evGx[ 80 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.E[ 60 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.E[ 64 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 68 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.E[ 72 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.E[ 76 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 20 ]), &(acadoWorkspace.E[ 80 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.d[ 24 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.evGx[ 96 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 84 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.E[ 88 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.E[ 92 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 96 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.E[ 100 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.E[ 104 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 108 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.d[ 28 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.evGx[ 112 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 112 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.E[ 116 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.E[ 120 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 124 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.E[ 128 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.E[ 132 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 136 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 28 ]), &(acadoWorkspace.E[ 140 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.d[ 32 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.evGx[ 128 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.E[ 144 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.E[ 148 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 152 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.E[ 156 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.E[ 160 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 164 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.E[ 168 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.E[ 172 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 32 ]), &(acadoWorkspace.E[ 176 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 36 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.evGx[ 144 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 180 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.E[ 184 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.E[ 188 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 192 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.E[ 196 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.E[ 200 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 204 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.E[ 208 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.E[ 212 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 216 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.d[ 40 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 160 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 220 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.E[ 224 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.E[ 228 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 232 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.E[ 236 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.E[ 240 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 244 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.E[ 248 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.E[ 252 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 256 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 40 ]), &(acadoWorkspace.E[ 260 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.d[ 44 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.evGx[ 176 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.E[ 264 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.E[ 268 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 272 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.E[ 276 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.E[ 280 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 284 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.E[ 288 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.E[ 292 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 296 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.E[ 300 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.E[ 304 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 44 ]), &(acadoWorkspace.E[ 308 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.d[ 48 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.evGx[ 192 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 312 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.E[ 316 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.E[ 320 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 324 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.E[ 328 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.E[ 332 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 336 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.E[ 340 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.E[ 344 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 348 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.E[ 352 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.E[ 356 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 360 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.d[ 52 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.evGx[ 208 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 364 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.E[ 368 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.E[ 372 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 376 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.E[ 380 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.E[ 384 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 388 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.E[ 392 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.E[ 396 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 400 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.E[ 404 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.E[ 408 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 412 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 52 ]), &(acadoWorkspace.E[ 416 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.d[ 56 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.evGx[ 224 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.E[ 420 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.E[ 424 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 428 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.E[ 432 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.E[ 436 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 440 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.E[ 444 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.E[ 448 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 452 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.E[ 456 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.E[ 460 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 464 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.E[ 468 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.E[ 472 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 56 ]), &(acadoWorkspace.E[ 476 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.d[ 60 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.evGx[ 240 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 480 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.E[ 484 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.E[ 488 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 492 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.E[ 496 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.E[ 500 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 504 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.E[ 508 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.E[ 512 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 516 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.E[ 520 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.E[ 524 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 528 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.E[ 532 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.E[ 536 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 540 ]) );
-
-acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) );
-acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) );
-
-acado_multGxGu( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.E, acadoWorkspace.QE );
-acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_zeroBlockH00(  );
-acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multCTQC( &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) );
-
-acado_zeroBlockH10( acadoWorkspace.H10 );
-acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.H10 );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 4 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 8 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 16 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 20 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 28 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 32 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 40 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 44 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.H10[ 44 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 44 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 44 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 44 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 44 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 52 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.H10[ 52 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 52 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 52 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 56 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.H10[ 56 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 56 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.H10[ 60 ]) );
-
-acadoWorkspace.H[4] = acadoWorkspace.H10[0];
-acadoWorkspace.H[5] = acadoWorkspace.H10[4];
-acadoWorkspace.H[6] = acadoWorkspace.H10[8];
-acadoWorkspace.H[7] = acadoWorkspace.H10[12];
-acadoWorkspace.H[8] = acadoWorkspace.H10[16];
-acadoWorkspace.H[9] = acadoWorkspace.H10[20];
-acadoWorkspace.H[10] = acadoWorkspace.H10[24];
-acadoWorkspace.H[11] = acadoWorkspace.H10[28];
-acadoWorkspace.H[12] = acadoWorkspace.H10[32];
-acadoWorkspace.H[13] = acadoWorkspace.H10[36];
-acadoWorkspace.H[14] = acadoWorkspace.H10[40];
-acadoWorkspace.H[15] = acadoWorkspace.H10[44];
-acadoWorkspace.H[16] = acadoWorkspace.H10[48];
-acadoWorkspace.H[17] = acadoWorkspace.H10[52];
-acadoWorkspace.H[18] = acadoWorkspace.H10[56];
-acadoWorkspace.H[19] = acadoWorkspace.H10[60];
-acadoWorkspace.H[24] = acadoWorkspace.H10[1];
-acadoWorkspace.H[25] = acadoWorkspace.H10[5];
-acadoWorkspace.H[26] = acadoWorkspace.H10[9];
-acadoWorkspace.H[27] = acadoWorkspace.H10[13];
-acadoWorkspace.H[28] = acadoWorkspace.H10[17];
-acadoWorkspace.H[29] = acadoWorkspace.H10[21];
-acadoWorkspace.H[30] = acadoWorkspace.H10[25];
-acadoWorkspace.H[31] = acadoWorkspace.H10[29];
-acadoWorkspace.H[32] = acadoWorkspace.H10[33];
-acadoWorkspace.H[33] = acadoWorkspace.H10[37];
-acadoWorkspace.H[34] = acadoWorkspace.H10[41];
-acadoWorkspace.H[35] = acadoWorkspace.H10[45];
-acadoWorkspace.H[36] = acadoWorkspace.H10[49];
-acadoWorkspace.H[37] = acadoWorkspace.H10[53];
-acadoWorkspace.H[38] = acadoWorkspace.H10[57];
-acadoWorkspace.H[39] = acadoWorkspace.H10[61];
-acadoWorkspace.H[44] = acadoWorkspace.H10[2];
-acadoWorkspace.H[45] = acadoWorkspace.H10[6];
-acadoWorkspace.H[46] = acadoWorkspace.H10[10];
-acadoWorkspace.H[47] = acadoWorkspace.H10[14];
-acadoWorkspace.H[48] = acadoWorkspace.H10[18];
-acadoWorkspace.H[49] = acadoWorkspace.H10[22];
-acadoWorkspace.H[50] = acadoWorkspace.H10[26];
-acadoWorkspace.H[51] = acadoWorkspace.H10[30];
-acadoWorkspace.H[52] = acadoWorkspace.H10[34];
-acadoWorkspace.H[53] = acadoWorkspace.H10[38];
-acadoWorkspace.H[54] = acadoWorkspace.H10[42];
-acadoWorkspace.H[55] = acadoWorkspace.H10[46];
-acadoWorkspace.H[56] = acadoWorkspace.H10[50];
-acadoWorkspace.H[57] = acadoWorkspace.H10[54];
-acadoWorkspace.H[58] = acadoWorkspace.H10[58];
-acadoWorkspace.H[59] = acadoWorkspace.H10[62];
-acadoWorkspace.H[64] = acadoWorkspace.H10[3];
-acadoWorkspace.H[65] = acadoWorkspace.H10[7];
-acadoWorkspace.H[66] = acadoWorkspace.H10[11];
-acadoWorkspace.H[67] = acadoWorkspace.H10[15];
-acadoWorkspace.H[68] = acadoWorkspace.H10[19];
-acadoWorkspace.H[69] = acadoWorkspace.H10[23];
-acadoWorkspace.H[70] = acadoWorkspace.H10[27];
-acadoWorkspace.H[71] = acadoWorkspace.H10[31];
-acadoWorkspace.H[72] = acadoWorkspace.H10[35];
-acadoWorkspace.H[73] = acadoWorkspace.H10[39];
-acadoWorkspace.H[74] = acadoWorkspace.H10[43];
-acadoWorkspace.H[75] = acadoWorkspace.H10[47];
-acadoWorkspace.H[76] = acadoWorkspace.H10[51];
-acadoWorkspace.H[77] = acadoWorkspace.H10[55];
-acadoWorkspace.H[78] = acadoWorkspace.H10[59];
-acadoWorkspace.H[79] = acadoWorkspace.H10[63];
-
-acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 );
-acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 4 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 40 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 112 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 220 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 364 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-
-acado_zeroBlockH11( 0, 1 );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QE[ 8 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 16 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 28 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 44 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 64 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 88 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 116 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 148 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 184 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 224 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 268 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 316 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 368 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 424 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 484 ]) );
-
-acado_zeroBlockH11( 0, 2 );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 20 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 32 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 68 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 92 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 152 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 188 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 272 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 320 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 428 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 488 ]) );
-
-acado_zeroBlockH11( 0, 3 );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 52 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 124 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 232 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 376 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) );
-
-acado_zeroBlockH11( 0, 4 );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 496 ]) );
-
-acado_zeroBlockH11( 0, 5 );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 0, 6 );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 0, 7 );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 0, 8 );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 0, 9 );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 0, 10 );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 0, 11 );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 0, 12 );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 0, 13 );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 0, 14 );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 0, 15 );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QE[ 8 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 16 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 28 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 44 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 64 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 88 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 116 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 148 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 184 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 224 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 268 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 316 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 368 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 424 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 484 ]) );
-
-acado_zeroBlockH11( 1, 2 );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QE[ 20 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 32 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 68 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 92 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 152 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 188 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 272 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 320 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 428 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 488 ]) );
-
-acado_zeroBlockH11( 1, 3 );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 52 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 124 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 232 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 376 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 492 ]) );
-
-acado_zeroBlockH11( 1, 4 );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 496 ]) );
-
-acado_zeroBlockH11( 1, 5 );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 1, 6 );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 1, 7 );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 1, 8 );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 1, 9 );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 1, 10 );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 1, 11 );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 1, 12 );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 1, 13 );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 1, 14 );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 1, 15 );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QE[ 20 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 32 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 68 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 92 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 152 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 188 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 272 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 320 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 428 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 488 ]) );
-
-acado_zeroBlockH11( 2, 3 );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 52 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 124 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 232 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 376 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 492 ]) );
-
-acado_zeroBlockH11( 2, 4 );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 496 ]) );
-
-acado_zeroBlockH11( 2, 5 );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 2, 6 );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 2, 7 );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 2, 8 );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 2, 9 );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 2, 10 );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 2, 11 );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 2, 12 );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 2, 13 );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 2, 14 );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 2, 15 );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 52 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 124 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 232 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 376 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-
-acado_zeroBlockH11( 3, 4 );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 496 ]) );
-
-acado_zeroBlockH11( 3, 5 );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 3, 6 );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 3, 7 );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 3, 8 );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 3, 9 );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 3, 10 );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 3, 11 );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 3, 12 );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 3, 13 );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 3, 14 );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 3, 15 );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QE[ 56 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 76 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 100 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 128 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 160 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 196 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 236 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 280 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 328 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 380 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 436 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 496 ]) );
-
-acado_zeroBlockH11( 4, 5 );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 4, 6 );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 4, 7 );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 4, 8 );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 4, 9 );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 4, 10 );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 4, 11 );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 4, 12 );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 4, 13 );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 4, 14 );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 4, 15 );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QE[ 80 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 104 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 164 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 200 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 284 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 332 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 440 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 500 ]) );
-
-acado_zeroBlockH11( 5, 6 );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 5, 7 );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 5, 8 );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 5, 9 );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 5, 10 );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 5, 11 );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 5, 12 );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 5, 13 );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 5, 14 );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 5, 15 );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 136 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 244 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 388 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-
-acado_zeroBlockH11( 6, 7 );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 6, 8 );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 6, 9 );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 6, 10 );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 6, 11 );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 6, 12 );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 6, 13 );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 6, 14 );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 6, 15 );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QE[ 140 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 172 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 208 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 248 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 292 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 340 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 392 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 448 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 508 ]) );
-
-acado_zeroBlockH11( 7, 8 );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 7, 9 );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 7, 10 );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 7, 11 );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 7, 12 );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 7, 13 );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 7, 14 );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 7, 15 );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QE[ 176 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 212 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 296 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 344 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 452 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 512 ]) );
-
-acado_zeroBlockH11( 8, 9 );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 8, 10 );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 8, 11 );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 8, 12 );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 8, 13 );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 8, 14 );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 8, 15 );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 256 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 400 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
-
-acado_zeroBlockH11( 9, 10 );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 9, 11 );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 9, 12 );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 9, 13 );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 9, 14 );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 9, 15 );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QE[ 260 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 304 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 352 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 404 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 460 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 520 ]) );
-
-acado_zeroBlockH11( 10, 11 );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 10, 12 );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 10, 13 );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 10, 14 );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 10, 15 );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QE[ 308 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 356 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 464 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 524 ]) );
-
-acado_zeroBlockH11( 11, 12 );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 11, 13 );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 11, 14 );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 11, 15 );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 412 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-
-acado_zeroBlockH11( 12, 13 );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 12, 14 );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 12, 15 );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QE[ 416 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 472 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 532 ]) );
-
-acado_zeroBlockH11( 13, 14 );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 13, 15 );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QE[ 476 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 536 ]) );
-
-acado_zeroBlockH11( 14, 15 );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-
-
-acado_copyHTH( 1, 0 );
-acado_copyHTH( 2, 0 );
-acado_copyHTH( 2, 1 );
-acado_copyHTH( 3, 0 );
-acado_copyHTH( 3, 1 );
-acado_copyHTH( 3, 2 );
-acado_copyHTH( 4, 0 );
-acado_copyHTH( 4, 1 );
-acado_copyHTH( 4, 2 );
-acado_copyHTH( 4, 3 );
-acado_copyHTH( 5, 0 );
-acado_copyHTH( 5, 1 );
-acado_copyHTH( 5, 2 );
-acado_copyHTH( 5, 3 );
-acado_copyHTH( 5, 4 );
-acado_copyHTH( 6, 0 );
-acado_copyHTH( 6, 1 );
-acado_copyHTH( 6, 2 );
-acado_copyHTH( 6, 3 );
-acado_copyHTH( 6, 4 );
-acado_copyHTH( 6, 5 );
-acado_copyHTH( 7, 0 );
-acado_copyHTH( 7, 1 );
-acado_copyHTH( 7, 2 );
-acado_copyHTH( 7, 3 );
-acado_copyHTH( 7, 4 );
-acado_copyHTH( 7, 5 );
-acado_copyHTH( 7, 6 );
-acado_copyHTH( 8, 0 );
-acado_copyHTH( 8, 1 );
-acado_copyHTH( 8, 2 );
-acado_copyHTH( 8, 3 );
-acado_copyHTH( 8, 4 );
-acado_copyHTH( 8, 5 );
-acado_copyHTH( 8, 6 );
-acado_copyHTH( 8, 7 );
-acado_copyHTH( 9, 0 );
-acado_copyHTH( 9, 1 );
-acado_copyHTH( 9, 2 );
-acado_copyHTH( 9, 3 );
-acado_copyHTH( 9, 4 );
-acado_copyHTH( 9, 5 );
-acado_copyHTH( 9, 6 );
-acado_copyHTH( 9, 7 );
-acado_copyHTH( 9, 8 );
-acado_copyHTH( 10, 0 );
-acado_copyHTH( 10, 1 );
-acado_copyHTH( 10, 2 );
-acado_copyHTH( 10, 3 );
-acado_copyHTH( 10, 4 );
-acado_copyHTH( 10, 5 );
-acado_copyHTH( 10, 6 );
-acado_copyHTH( 10, 7 );
-acado_copyHTH( 10, 8 );
-acado_copyHTH( 10, 9 );
-acado_copyHTH( 11, 0 );
-acado_copyHTH( 11, 1 );
-acado_copyHTH( 11, 2 );
-acado_copyHTH( 11, 3 );
-acado_copyHTH( 11, 4 );
-acado_copyHTH( 11, 5 );
-acado_copyHTH( 11, 6 );
-acado_copyHTH( 11, 7 );
-acado_copyHTH( 11, 8 );
-acado_copyHTH( 11, 9 );
-acado_copyHTH( 11, 10 );
-acado_copyHTH( 12, 0 );
-acado_copyHTH( 12, 1 );
-acado_copyHTH( 12, 2 );
-acado_copyHTH( 12, 3 );
-acado_copyHTH( 12, 4 );
-acado_copyHTH( 12, 5 );
-acado_copyHTH( 12, 6 );
-acado_copyHTH( 12, 7 );
-acado_copyHTH( 12, 8 );
-acado_copyHTH( 12, 9 );
-acado_copyHTH( 12, 10 );
-acado_copyHTH( 12, 11 );
-acado_copyHTH( 13, 0 );
-acado_copyHTH( 13, 1 );
-acado_copyHTH( 13, 2 );
-acado_copyHTH( 13, 3 );
-acado_copyHTH( 13, 4 );
-acado_copyHTH( 13, 5 );
-acado_copyHTH( 13, 6 );
-acado_copyHTH( 13, 7 );
-acado_copyHTH( 13, 8 );
-acado_copyHTH( 13, 9 );
-acado_copyHTH( 13, 10 );
-acado_copyHTH( 13, 11 );
-acado_copyHTH( 13, 12 );
-acado_copyHTH( 14, 0 );
-acado_copyHTH( 14, 1 );
-acado_copyHTH( 14, 2 );
-acado_copyHTH( 14, 3 );
-acado_copyHTH( 14, 4 );
-acado_copyHTH( 14, 5 );
-acado_copyHTH( 14, 6 );
-acado_copyHTH( 14, 7 );
-acado_copyHTH( 14, 8 );
-acado_copyHTH( 14, 9 );
-acado_copyHTH( 14, 10 );
-acado_copyHTH( 14, 11 );
-acado_copyHTH( 14, 12 );
-acado_copyHTH( 14, 13 );
-acado_copyHTH( 15, 0 );
-acado_copyHTH( 15, 1 );
-acado_copyHTH( 15, 2 );
-acado_copyHTH( 15, 3 );
-acado_copyHTH( 15, 4 );
-acado_copyHTH( 15, 5 );
-acado_copyHTH( 15, 6 );
-acado_copyHTH( 15, 7 );
-acado_copyHTH( 15, 8 );
-acado_copyHTH( 15, 9 );
-acado_copyHTH( 15, 10 );
-acado_copyHTH( 15, 11 );
-acado_copyHTH( 15, 12 );
-acado_copyHTH( 15, 13 );
-acado_copyHTH( 15, 14 );
-
-acadoWorkspace.H[80] = acadoWorkspace.H10[0];
-acadoWorkspace.H[81] = acadoWorkspace.H10[1];
-acadoWorkspace.H[82] = acadoWorkspace.H10[2];
-acadoWorkspace.H[83] = acadoWorkspace.H10[3];
-acadoWorkspace.H[100] = acadoWorkspace.H10[4];
-acadoWorkspace.H[101] = acadoWorkspace.H10[5];
-acadoWorkspace.H[102] = acadoWorkspace.H10[6];
-acadoWorkspace.H[103] = acadoWorkspace.H10[7];
-acadoWorkspace.H[120] = acadoWorkspace.H10[8];
-acadoWorkspace.H[121] = acadoWorkspace.H10[9];
-acadoWorkspace.H[122] = acadoWorkspace.H10[10];
-acadoWorkspace.H[123] = acadoWorkspace.H10[11];
-acadoWorkspace.H[140] = acadoWorkspace.H10[12];
-acadoWorkspace.H[141] = acadoWorkspace.H10[13];
-acadoWorkspace.H[142] = acadoWorkspace.H10[14];
-acadoWorkspace.H[143] = acadoWorkspace.H10[15];
-acadoWorkspace.H[160] = acadoWorkspace.H10[16];
-acadoWorkspace.H[161] = acadoWorkspace.H10[17];
-acadoWorkspace.H[162] = acadoWorkspace.H10[18];
-acadoWorkspace.H[163] = acadoWorkspace.H10[19];
-acadoWorkspace.H[180] = acadoWorkspace.H10[20];
-acadoWorkspace.H[181] = acadoWorkspace.H10[21];
-acadoWorkspace.H[182] = acadoWorkspace.H10[22];
-acadoWorkspace.H[183] = acadoWorkspace.H10[23];
-acadoWorkspace.H[200] = acadoWorkspace.H10[24];
-acadoWorkspace.H[201] = acadoWorkspace.H10[25];
-acadoWorkspace.H[202] = acadoWorkspace.H10[26];
-acadoWorkspace.H[203] = acadoWorkspace.H10[27];
-acadoWorkspace.H[220] = acadoWorkspace.H10[28];
-acadoWorkspace.H[221] = acadoWorkspace.H10[29];
-acadoWorkspace.H[222] = acadoWorkspace.H10[30];
-acadoWorkspace.H[223] = acadoWorkspace.H10[31];
-acadoWorkspace.H[240] = acadoWorkspace.H10[32];
-acadoWorkspace.H[241] = acadoWorkspace.H10[33];
-acadoWorkspace.H[242] = acadoWorkspace.H10[34];
-acadoWorkspace.H[243] = acadoWorkspace.H10[35];
-acadoWorkspace.H[260] = acadoWorkspace.H10[36];
-acadoWorkspace.H[261] = acadoWorkspace.H10[37];
-acadoWorkspace.H[262] = acadoWorkspace.H10[38];
-acadoWorkspace.H[263] = acadoWorkspace.H10[39];
-acadoWorkspace.H[280] = acadoWorkspace.H10[40];
-acadoWorkspace.H[281] = acadoWorkspace.H10[41];
-acadoWorkspace.H[282] = acadoWorkspace.H10[42];
-acadoWorkspace.H[283] = acadoWorkspace.H10[43];
-acadoWorkspace.H[300] = acadoWorkspace.H10[44];
-acadoWorkspace.H[301] = acadoWorkspace.H10[45];
-acadoWorkspace.H[302] = acadoWorkspace.H10[46];
-acadoWorkspace.H[303] = acadoWorkspace.H10[47];
-acadoWorkspace.H[320] = acadoWorkspace.H10[48];
-acadoWorkspace.H[321] = acadoWorkspace.H10[49];
-acadoWorkspace.H[322] = acadoWorkspace.H10[50];
-acadoWorkspace.H[323] = acadoWorkspace.H10[51];
-acadoWorkspace.H[340] = acadoWorkspace.H10[52];
-acadoWorkspace.H[341] = acadoWorkspace.H10[53];
-acadoWorkspace.H[342] = acadoWorkspace.H10[54];
-acadoWorkspace.H[343] = acadoWorkspace.H10[55];
-acadoWorkspace.H[360] = acadoWorkspace.H10[56];
-acadoWorkspace.H[361] = acadoWorkspace.H10[57];
-acadoWorkspace.H[362] = acadoWorkspace.H10[58];
-acadoWorkspace.H[363] = acadoWorkspace.H10[59];
-acadoWorkspace.H[380] = acadoWorkspace.H10[60];
-acadoWorkspace.H[381] = acadoWorkspace.H10[61];
-acadoWorkspace.H[382] = acadoWorkspace.H10[62];
-acadoWorkspace.H[383] = acadoWorkspace.H10[63];
-
-acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd );
-acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.Qd[ 8 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.Qd[ 16 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.Qd[ 20 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.Qd[ 28 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.Qd[ 32 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.Qd[ 40 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.Qd[ 44 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) );
-acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) );
-
-acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g );
-acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 4 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 40 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 112 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 220 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 364 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 8 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 16 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 28 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 44 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 64 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 88 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 116 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 148 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 184 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 224 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 268 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 316 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 368 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 424 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 484 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 20 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 32 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 68 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 92 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 152 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 188 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 272 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 320 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 428 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 488 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 52 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 124 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 232 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 376 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 56 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 76 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 100 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 128 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 160 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 196 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 236 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 280 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 328 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 380 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 436 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 496 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 80 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 104 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 164 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 200 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 284 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 332 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 440 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 500 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 136 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 244 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 388 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 140 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 172 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 208 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 248 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 292 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 340 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 392 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 448 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 508 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 176 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 212 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 296 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 344 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 452 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 512 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 256 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 400 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 260 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 304 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 352 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 404 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 460 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 520 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 308 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 356 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 464 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 524 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 412 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 416 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 472 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 532 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 476 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 536 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 19 ]) );
-acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-lRun3 = xBoundIndices[ lRun1 ] - 4;
-lRun4 = ((lRun3) / (4)) + (1);
-acadoWorkspace.A[lRun1 * 20] = acadoWorkspace.evGx[lRun3 * 4];
-acadoWorkspace.A[lRun1 * 20 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1];
-acadoWorkspace.A[lRun1 * 20 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2];
-acadoWorkspace.A[lRun1 * 20 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3];
-for (lRun2 = 0; lRun2 < lRun4; ++lRun2)
-{
-lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4));
-acadoWorkspace.A[(lRun1 * 20) + (lRun2 + 4)] = acadoWorkspace.E[lRun5];
-}
-}
-
-}
-
-void acado_condenseFdb(  )
-{
-real_t tmp;
-
-acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0];
-acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1];
-acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2];
-acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3];
-
-acadoWorkspace.Dy[0] -= acadoVariables.y[0];
-acadoWorkspace.Dy[1] -= acadoVariables.y[1];
-acadoWorkspace.Dy[2] -= acadoVariables.y[2];
-acadoWorkspace.Dy[3] -= acadoVariables.y[3];
-acadoWorkspace.Dy[4] -= acadoVariables.y[4];
-acadoWorkspace.Dy[5] -= acadoVariables.y[5];
-acadoWorkspace.Dy[6] -= acadoVariables.y[6];
-acadoWorkspace.Dy[7] -= acadoVariables.y[7];
-acadoWorkspace.Dy[8] -= acadoVariables.y[8];
-acadoWorkspace.Dy[9] -= acadoVariables.y[9];
-acadoWorkspace.Dy[10] -= acadoVariables.y[10];
-acadoWorkspace.Dy[11] -= acadoVariables.y[11];
-acadoWorkspace.Dy[12] -= acadoVariables.y[12];
-acadoWorkspace.Dy[13] -= acadoVariables.y[13];
-acadoWorkspace.Dy[14] -= acadoVariables.y[14];
-acadoWorkspace.Dy[15] -= acadoVariables.y[15];
-acadoWorkspace.Dy[16] -= acadoVariables.y[16];
-acadoWorkspace.Dy[17] -= acadoVariables.y[17];
-acadoWorkspace.Dy[18] -= acadoVariables.y[18];
-acadoWorkspace.Dy[19] -= acadoVariables.y[19];
-acadoWorkspace.Dy[20] -= acadoVariables.y[20];
-acadoWorkspace.Dy[21] -= acadoVariables.y[21];
-acadoWorkspace.Dy[22] -= acadoVariables.y[22];
-acadoWorkspace.Dy[23] -= acadoVariables.y[23];
-acadoWorkspace.Dy[24] -= acadoVariables.y[24];
-acadoWorkspace.Dy[25] -= acadoVariables.y[25];
-acadoWorkspace.Dy[26] -= acadoVariables.y[26];
-acadoWorkspace.Dy[27] -= acadoVariables.y[27];
-acadoWorkspace.Dy[28] -= acadoVariables.y[28];
-acadoWorkspace.Dy[29] -= acadoVariables.y[29];
-acadoWorkspace.Dy[30] -= acadoVariables.y[30];
-acadoWorkspace.Dy[31] -= acadoVariables.y[31];
-acadoWorkspace.Dy[32] -= acadoVariables.y[32];
-acadoWorkspace.Dy[33] -= acadoVariables.y[33];
-acadoWorkspace.Dy[34] -= acadoVariables.y[34];
-acadoWorkspace.Dy[35] -= acadoVariables.y[35];
-acadoWorkspace.Dy[36] -= acadoVariables.y[36];
-acadoWorkspace.Dy[37] -= acadoVariables.y[37];
-acadoWorkspace.Dy[38] -= acadoVariables.y[38];
-acadoWorkspace.Dy[39] -= acadoVariables.y[39];
-acadoWorkspace.Dy[40] -= acadoVariables.y[40];
-acadoWorkspace.Dy[41] -= acadoVariables.y[41];
-acadoWorkspace.Dy[42] -= acadoVariables.y[42];
-acadoWorkspace.Dy[43] -= acadoVariables.y[43];
-acadoWorkspace.Dy[44] -= acadoVariables.y[44];
-acadoWorkspace.Dy[45] -= acadoVariables.y[45];
-acadoWorkspace.Dy[46] -= acadoVariables.y[46];
-acadoWorkspace.Dy[47] -= acadoVariables.y[47];
-acadoWorkspace.DyN[0] -= acadoVariables.yN[0];
-acadoWorkspace.DyN[1] -= acadoVariables.yN[1];
-
-acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 3 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 6 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 9 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 15 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 18 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 21 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 27 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 33 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 39 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 42 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 45 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 19 ]) );
-
-acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy );
-acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 3 ]), &(acadoWorkspace.QDy[ 4 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 6 ]), &(acadoWorkspace.QDy[ 8 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 9 ]), &(acadoWorkspace.QDy[ 12 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 16 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 20 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 18 ]), &(acadoWorkspace.QDy[ 24 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 21 ]), &(acadoWorkspace.QDy[ 28 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 32 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 27 ]), &(acadoWorkspace.QDy[ 36 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 40 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 33 ]), &(acadoWorkspace.QDy[ 44 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 48 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 39 ]), &(acadoWorkspace.QDy[ 52 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 42 ]), &(acadoWorkspace.QDy[ 56 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 60 ]) );
-
-acadoWorkspace.QDy[64] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1];
-acadoWorkspace.QDy[65] = + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[1];
-acadoWorkspace.QDy[66] = + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[1];
-acadoWorkspace.QDy[67] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1];
-
-acadoWorkspace.QDy[4] += acadoWorkspace.Qd[0];
-acadoWorkspace.QDy[5] += acadoWorkspace.Qd[1];
-acadoWorkspace.QDy[6] += acadoWorkspace.Qd[2];
-acadoWorkspace.QDy[7] += acadoWorkspace.Qd[3];
-acadoWorkspace.QDy[8] += acadoWorkspace.Qd[4];
-acadoWorkspace.QDy[9] += acadoWorkspace.Qd[5];
-acadoWorkspace.QDy[10] += acadoWorkspace.Qd[6];
-acadoWorkspace.QDy[11] += acadoWorkspace.Qd[7];
-acadoWorkspace.QDy[12] += acadoWorkspace.Qd[8];
-acadoWorkspace.QDy[13] += acadoWorkspace.Qd[9];
-acadoWorkspace.QDy[14] += acadoWorkspace.Qd[10];
-acadoWorkspace.QDy[15] += acadoWorkspace.Qd[11];
-acadoWorkspace.QDy[16] += acadoWorkspace.Qd[12];
-acadoWorkspace.QDy[17] += acadoWorkspace.Qd[13];
-acadoWorkspace.QDy[18] += acadoWorkspace.Qd[14];
-acadoWorkspace.QDy[19] += acadoWorkspace.Qd[15];
-acadoWorkspace.QDy[20] += acadoWorkspace.Qd[16];
-acadoWorkspace.QDy[21] += acadoWorkspace.Qd[17];
-acadoWorkspace.QDy[22] += acadoWorkspace.Qd[18];
-acadoWorkspace.QDy[23] += acadoWorkspace.Qd[19];
-acadoWorkspace.QDy[24] += acadoWorkspace.Qd[20];
-acadoWorkspace.QDy[25] += acadoWorkspace.Qd[21];
-acadoWorkspace.QDy[26] += acadoWorkspace.Qd[22];
-acadoWorkspace.QDy[27] += acadoWorkspace.Qd[23];
-acadoWorkspace.QDy[28] += acadoWorkspace.Qd[24];
-acadoWorkspace.QDy[29] += acadoWorkspace.Qd[25];
-acadoWorkspace.QDy[30] += acadoWorkspace.Qd[26];
-acadoWorkspace.QDy[31] += acadoWorkspace.Qd[27];
-acadoWorkspace.QDy[32] += acadoWorkspace.Qd[28];
-acadoWorkspace.QDy[33] += acadoWorkspace.Qd[29];
-acadoWorkspace.QDy[34] += acadoWorkspace.Qd[30];
-acadoWorkspace.QDy[35] += acadoWorkspace.Qd[31];
-acadoWorkspace.QDy[36] += acadoWorkspace.Qd[32];
-acadoWorkspace.QDy[37] += acadoWorkspace.Qd[33];
-acadoWorkspace.QDy[38] += acadoWorkspace.Qd[34];
-acadoWorkspace.QDy[39] += acadoWorkspace.Qd[35];
-acadoWorkspace.QDy[40] += acadoWorkspace.Qd[36];
-acadoWorkspace.QDy[41] += acadoWorkspace.Qd[37];
-acadoWorkspace.QDy[42] += acadoWorkspace.Qd[38];
-acadoWorkspace.QDy[43] += acadoWorkspace.Qd[39];
-acadoWorkspace.QDy[44] += acadoWorkspace.Qd[40];
-acadoWorkspace.QDy[45] += acadoWorkspace.Qd[41];
-acadoWorkspace.QDy[46] += acadoWorkspace.Qd[42];
-acadoWorkspace.QDy[47] += acadoWorkspace.Qd[43];
-acadoWorkspace.QDy[48] += acadoWorkspace.Qd[44];
-acadoWorkspace.QDy[49] += acadoWorkspace.Qd[45];
-acadoWorkspace.QDy[50] += acadoWorkspace.Qd[46];
-acadoWorkspace.QDy[51] += acadoWorkspace.Qd[47];
-acadoWorkspace.QDy[52] += acadoWorkspace.Qd[48];
-acadoWorkspace.QDy[53] += acadoWorkspace.Qd[49];
-acadoWorkspace.QDy[54] += acadoWorkspace.Qd[50];
-acadoWorkspace.QDy[55] += acadoWorkspace.Qd[51];
-acadoWorkspace.QDy[56] += acadoWorkspace.Qd[52];
-acadoWorkspace.QDy[57] += acadoWorkspace.Qd[53];
-acadoWorkspace.QDy[58] += acadoWorkspace.Qd[54];
-acadoWorkspace.QDy[59] += acadoWorkspace.Qd[55];
-acadoWorkspace.QDy[60] += acadoWorkspace.Qd[56];
-acadoWorkspace.QDy[61] += acadoWorkspace.Qd[57];
-acadoWorkspace.QDy[62] += acadoWorkspace.Qd[58];
-acadoWorkspace.QDy[63] += acadoWorkspace.Qd[59];
-acadoWorkspace.QDy[64] += acadoWorkspace.Qd[60];
-acadoWorkspace.QDy[65] += acadoWorkspace.Qd[61];
-acadoWorkspace.QDy[66] += acadoWorkspace.Qd[62];
-acadoWorkspace.QDy[67] += acadoWorkspace.Qd[63];
-
-acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67];
-acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67];
-acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67];
-acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67];
-
-
-acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 4 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.QDy[ 8 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 16 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.QDy[ 20 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 28 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.QDy[ 32 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 40 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.QDy[ 44 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 52 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.QDy[ 56 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 64 ]), &(acadoWorkspace.g[ 19 ]) );
-
-acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3];
-acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3];
-tmp = acadoVariables.x[6] + acadoWorkspace.d[2];
-acadoWorkspace.lbA[0] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[0] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[7] + acadoWorkspace.d[3];
-acadoWorkspace.lbA[1] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[1] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[10] + acadoWorkspace.d[6];
-acadoWorkspace.lbA[2] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[2] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[11] + acadoWorkspace.d[7];
-acadoWorkspace.lbA[3] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[3] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[14] + acadoWorkspace.d[10];
-acadoWorkspace.lbA[4] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[4] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[15] + acadoWorkspace.d[11];
-acadoWorkspace.lbA[5] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[5] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[18] + acadoWorkspace.d[14];
-acadoWorkspace.lbA[6] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[6] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[19] + acadoWorkspace.d[15];
-acadoWorkspace.lbA[7] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[7] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[22] + acadoWorkspace.d[18];
-acadoWorkspace.lbA[8] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[8] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[23] + acadoWorkspace.d[19];
-acadoWorkspace.lbA[9] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[9] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[26] + acadoWorkspace.d[22];
-acadoWorkspace.lbA[10] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[10] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[27] + acadoWorkspace.d[23];
-acadoWorkspace.lbA[11] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[11] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[30] + acadoWorkspace.d[26];
-acadoWorkspace.lbA[12] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[12] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[31] + acadoWorkspace.d[27];
-acadoWorkspace.lbA[13] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[13] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[34] + acadoWorkspace.d[30];
-acadoWorkspace.lbA[14] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[14] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[35] + acadoWorkspace.d[31];
-acadoWorkspace.lbA[15] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[15] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[38] + acadoWorkspace.d[34];
-acadoWorkspace.lbA[16] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[16] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[39] + acadoWorkspace.d[35];
-acadoWorkspace.lbA[17] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[17] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[42] + acadoWorkspace.d[38];
-acadoWorkspace.lbA[18] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[18] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[43] + acadoWorkspace.d[39];
-acadoWorkspace.lbA[19] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[19] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[46] + acadoWorkspace.d[42];
-acadoWorkspace.lbA[20] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[20] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[47] + acadoWorkspace.d[43];
-acadoWorkspace.lbA[21] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[21] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[50] + acadoWorkspace.d[46];
-acadoWorkspace.lbA[22] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[22] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[51] + acadoWorkspace.d[47];
-acadoWorkspace.lbA[23] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[23] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[54] + acadoWorkspace.d[50];
-acadoWorkspace.lbA[24] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[24] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[55] + acadoWorkspace.d[51];
-acadoWorkspace.lbA[25] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[25] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[58] + acadoWorkspace.d[54];
-acadoWorkspace.lbA[26] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[26] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[59] + acadoWorkspace.d[55];
-acadoWorkspace.lbA[27] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[27] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[62] + acadoWorkspace.d[58];
-acadoWorkspace.lbA[28] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[28] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[63] + acadoWorkspace.d[59];
-acadoWorkspace.lbA[29] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[29] = (real_t)8.7266462599716477e-01 - tmp;
-tmp = acadoVariables.x[66] + acadoWorkspace.d[62];
-acadoWorkspace.lbA[30] = (real_t)-1.5707963267948966e+00 - tmp;
-acadoWorkspace.ubA[30] = (real_t)1.5707963267948966e+00 - tmp;
-tmp = acadoVariables.x[67] + acadoWorkspace.d[63];
-acadoWorkspace.lbA[31] = (real_t)-8.7266462599716477e-01 - tmp;
-acadoWorkspace.ubA[31] = (real_t)8.7266462599716477e-01 - tmp;
-
-}
-
-void acado_expand(  )
-{
-acadoVariables.x[0] += acadoWorkspace.x[0];
-acadoVariables.x[1] += acadoWorkspace.x[1];
-acadoVariables.x[2] += acadoWorkspace.x[2];
-acadoVariables.x[3] += acadoWorkspace.x[3];
-
-acadoVariables.u[0] += acadoWorkspace.x[4];
-acadoVariables.u[1] += acadoWorkspace.x[5];
-acadoVariables.u[2] += acadoWorkspace.x[6];
-acadoVariables.u[3] += acadoWorkspace.x[7];
-acadoVariables.u[4] += acadoWorkspace.x[8];
-acadoVariables.u[5] += acadoWorkspace.x[9];
-acadoVariables.u[6] += acadoWorkspace.x[10];
-acadoVariables.u[7] += acadoWorkspace.x[11];
-acadoVariables.u[8] += acadoWorkspace.x[12];
-acadoVariables.u[9] += acadoWorkspace.x[13];
-acadoVariables.u[10] += acadoWorkspace.x[14];
-acadoVariables.u[11] += acadoWorkspace.x[15];
-acadoVariables.u[12] += acadoWorkspace.x[16];
-acadoVariables.u[13] += acadoWorkspace.x[17];
-acadoVariables.u[14] += acadoWorkspace.x[18];
-acadoVariables.u[15] += acadoWorkspace.x[19];
-
-acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0];
-acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1];
-acadoVariables.x[6] += + acadoWorkspace.evGx[8]*acadoWorkspace.x[0] + acadoWorkspace.evGx[9]*acadoWorkspace.x[1] + acadoWorkspace.evGx[10]*acadoWorkspace.x[2] + acadoWorkspace.evGx[11]*acadoWorkspace.x[3] + acadoWorkspace.d[2];
-acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.d[3];
-acadoVariables.x[8] += + acadoWorkspace.evGx[16]*acadoWorkspace.x[0] + acadoWorkspace.evGx[17]*acadoWorkspace.x[1] + acadoWorkspace.evGx[18]*acadoWorkspace.x[2] + acadoWorkspace.evGx[19]*acadoWorkspace.x[3] + acadoWorkspace.d[4];
-acadoVariables.x[9] += + acadoWorkspace.evGx[20]*acadoWorkspace.x[0] + acadoWorkspace.evGx[21]*acadoWorkspace.x[1] + acadoWorkspace.evGx[22]*acadoWorkspace.x[2] + acadoWorkspace.evGx[23]*acadoWorkspace.x[3] + acadoWorkspace.d[5];
-acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.d[6];
-acadoVariables.x[11] += + acadoWorkspace.evGx[28]*acadoWorkspace.x[0] + acadoWorkspace.evGx[29]*acadoWorkspace.x[1] + acadoWorkspace.evGx[30]*acadoWorkspace.x[2] + acadoWorkspace.evGx[31]*acadoWorkspace.x[3] + acadoWorkspace.d[7];
-acadoVariables.x[12] += + acadoWorkspace.evGx[32]*acadoWorkspace.x[0] + acadoWorkspace.evGx[33]*acadoWorkspace.x[1] + acadoWorkspace.evGx[34]*acadoWorkspace.x[2] + acadoWorkspace.evGx[35]*acadoWorkspace.x[3] + acadoWorkspace.d[8];
-acadoVariables.x[13] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.d[9];
-acadoVariables.x[14] += + acadoWorkspace.evGx[40]*acadoWorkspace.x[0] + acadoWorkspace.evGx[41]*acadoWorkspace.x[1] + acadoWorkspace.evGx[42]*acadoWorkspace.x[2] + acadoWorkspace.evGx[43]*acadoWorkspace.x[3] + acadoWorkspace.d[10];
-acadoVariables.x[15] += + acadoWorkspace.evGx[44]*acadoWorkspace.x[0] + acadoWorkspace.evGx[45]*acadoWorkspace.x[1] + acadoWorkspace.evGx[46]*acadoWorkspace.x[2] + acadoWorkspace.evGx[47]*acadoWorkspace.x[3] + acadoWorkspace.d[11];
-acadoVariables.x[16] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.d[12];
-acadoVariables.x[17] += + acadoWorkspace.evGx[52]*acadoWorkspace.x[0] + acadoWorkspace.evGx[53]*acadoWorkspace.x[1] + acadoWorkspace.evGx[54]*acadoWorkspace.x[2] + acadoWorkspace.evGx[55]*acadoWorkspace.x[3] + acadoWorkspace.d[13];
-acadoVariables.x[18] += + acadoWorkspace.evGx[56]*acadoWorkspace.x[0] + acadoWorkspace.evGx[57]*acadoWorkspace.x[1] + acadoWorkspace.evGx[58]*acadoWorkspace.x[2] + acadoWorkspace.evGx[59]*acadoWorkspace.x[3] + acadoWorkspace.d[14];
-acadoVariables.x[19] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.d[15];
-acadoVariables.x[20] += + acadoWorkspace.evGx[64]*acadoWorkspace.x[0] + acadoWorkspace.evGx[65]*acadoWorkspace.x[1] + acadoWorkspace.evGx[66]*acadoWorkspace.x[2] + acadoWorkspace.evGx[67]*acadoWorkspace.x[3] + acadoWorkspace.d[16];
-acadoVariables.x[21] += + acadoWorkspace.evGx[68]*acadoWorkspace.x[0] + acadoWorkspace.evGx[69]*acadoWorkspace.x[1] + acadoWorkspace.evGx[70]*acadoWorkspace.x[2] + acadoWorkspace.evGx[71]*acadoWorkspace.x[3] + acadoWorkspace.d[17];
-acadoVariables.x[22] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.d[18];
-acadoVariables.x[23] += + acadoWorkspace.evGx[76]*acadoWorkspace.x[0] + acadoWorkspace.evGx[77]*acadoWorkspace.x[1] + acadoWorkspace.evGx[78]*acadoWorkspace.x[2] + acadoWorkspace.evGx[79]*acadoWorkspace.x[3] + acadoWorkspace.d[19];
-acadoVariables.x[24] += + acadoWorkspace.evGx[80]*acadoWorkspace.x[0] + acadoWorkspace.evGx[81]*acadoWorkspace.x[1] + acadoWorkspace.evGx[82]*acadoWorkspace.x[2] + acadoWorkspace.evGx[83]*acadoWorkspace.x[3] + acadoWorkspace.d[20];
-acadoVariables.x[25] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.d[21];
-acadoVariables.x[26] += + acadoWorkspace.evGx[88]*acadoWorkspace.x[0] + acadoWorkspace.evGx[89]*acadoWorkspace.x[1] + acadoWorkspace.evGx[90]*acadoWorkspace.x[2] + acadoWorkspace.evGx[91]*acadoWorkspace.x[3] + acadoWorkspace.d[22];
-acadoVariables.x[27] += + acadoWorkspace.evGx[92]*acadoWorkspace.x[0] + acadoWorkspace.evGx[93]*acadoWorkspace.x[1] + acadoWorkspace.evGx[94]*acadoWorkspace.x[2] + acadoWorkspace.evGx[95]*acadoWorkspace.x[3] + acadoWorkspace.d[23];
-acadoVariables.x[28] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.d[24];
-acadoVariables.x[29] += + acadoWorkspace.evGx[100]*acadoWorkspace.x[0] + acadoWorkspace.evGx[101]*acadoWorkspace.x[1] + acadoWorkspace.evGx[102]*acadoWorkspace.x[2] + acadoWorkspace.evGx[103]*acadoWorkspace.x[3] + acadoWorkspace.d[25];
-acadoVariables.x[30] += + acadoWorkspace.evGx[104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[107]*acadoWorkspace.x[3] + acadoWorkspace.d[26];
-acadoVariables.x[31] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.d[27];
-acadoVariables.x[32] += + acadoWorkspace.evGx[112]*acadoWorkspace.x[0] + acadoWorkspace.evGx[113]*acadoWorkspace.x[1] + acadoWorkspace.evGx[114]*acadoWorkspace.x[2] + acadoWorkspace.evGx[115]*acadoWorkspace.x[3] + acadoWorkspace.d[28];
-acadoVariables.x[33] += + acadoWorkspace.evGx[116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[119]*acadoWorkspace.x[3] + acadoWorkspace.d[29];
-acadoVariables.x[34] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.d[30];
-acadoVariables.x[35] += + acadoWorkspace.evGx[124]*acadoWorkspace.x[0] + acadoWorkspace.evGx[125]*acadoWorkspace.x[1] + acadoWorkspace.evGx[126]*acadoWorkspace.x[2] + acadoWorkspace.evGx[127]*acadoWorkspace.x[3] + acadoWorkspace.d[31];
-acadoVariables.x[36] += + acadoWorkspace.evGx[128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[131]*acadoWorkspace.x[3] + acadoWorkspace.d[32];
-acadoVariables.x[37] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.d[33];
-acadoVariables.x[38] += + acadoWorkspace.evGx[136]*acadoWorkspace.x[0] + acadoWorkspace.evGx[137]*acadoWorkspace.x[1] + acadoWorkspace.evGx[138]*acadoWorkspace.x[2] + acadoWorkspace.evGx[139]*acadoWorkspace.x[3] + acadoWorkspace.d[34];
-acadoVariables.x[39] += + acadoWorkspace.evGx[140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[143]*acadoWorkspace.x[3] + acadoWorkspace.d[35];
-acadoVariables.x[40] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.d[36];
-acadoVariables.x[41] += + acadoWorkspace.evGx[148]*acadoWorkspace.x[0] + acadoWorkspace.evGx[149]*acadoWorkspace.x[1] + acadoWorkspace.evGx[150]*acadoWorkspace.x[2] + acadoWorkspace.evGx[151]*acadoWorkspace.x[3] + acadoWorkspace.d[37];
-acadoVariables.x[42] += + acadoWorkspace.evGx[152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[155]*acadoWorkspace.x[3] + acadoWorkspace.d[38];
-acadoVariables.x[43] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.d[39];
-acadoVariables.x[44] += + acadoWorkspace.evGx[160]*acadoWorkspace.x[0] + acadoWorkspace.evGx[161]*acadoWorkspace.x[1] + acadoWorkspace.evGx[162]*acadoWorkspace.x[2] + acadoWorkspace.evGx[163]*acadoWorkspace.x[3] + acadoWorkspace.d[40];
-acadoVariables.x[45] += + acadoWorkspace.evGx[164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[167]*acadoWorkspace.x[3] + acadoWorkspace.d[41];
-acadoVariables.x[46] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.d[42];
-acadoVariables.x[47] += + acadoWorkspace.evGx[172]*acadoWorkspace.x[0] + acadoWorkspace.evGx[173]*acadoWorkspace.x[1] + acadoWorkspace.evGx[174]*acadoWorkspace.x[2] + acadoWorkspace.evGx[175]*acadoWorkspace.x[3] + acadoWorkspace.d[43];
-acadoVariables.x[48] += + acadoWorkspace.evGx[176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[179]*acadoWorkspace.x[3] + acadoWorkspace.d[44];
-acadoVariables.x[49] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.d[45];
-acadoVariables.x[50] += + acadoWorkspace.evGx[184]*acadoWorkspace.x[0] + acadoWorkspace.evGx[185]*acadoWorkspace.x[1] + acadoWorkspace.evGx[186]*acadoWorkspace.x[2] + acadoWorkspace.evGx[187]*acadoWorkspace.x[3] + acadoWorkspace.d[46];
-acadoVariables.x[51] += + acadoWorkspace.evGx[188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[191]*acadoWorkspace.x[3] + acadoWorkspace.d[47];
-acadoVariables.x[52] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.d[48];
-acadoVariables.x[53] += + acadoWorkspace.evGx[196]*acadoWorkspace.x[0] + acadoWorkspace.evGx[197]*acadoWorkspace.x[1] + acadoWorkspace.evGx[198]*acadoWorkspace.x[2] + acadoWorkspace.evGx[199]*acadoWorkspace.x[3] + acadoWorkspace.d[49];
-acadoVariables.x[54] += + acadoWorkspace.evGx[200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[203]*acadoWorkspace.x[3] + acadoWorkspace.d[50];
-acadoVariables.x[55] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.d[51];
-acadoVariables.x[56] += + acadoWorkspace.evGx[208]*acadoWorkspace.x[0] + acadoWorkspace.evGx[209]*acadoWorkspace.x[1] + acadoWorkspace.evGx[210]*acadoWorkspace.x[2] + acadoWorkspace.evGx[211]*acadoWorkspace.x[3] + acadoWorkspace.d[52];
-acadoVariables.x[57] += + acadoWorkspace.evGx[212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[215]*acadoWorkspace.x[3] + acadoWorkspace.d[53];
-acadoVariables.x[58] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.d[54];
-acadoVariables.x[59] += + acadoWorkspace.evGx[220]*acadoWorkspace.x[0] + acadoWorkspace.evGx[221]*acadoWorkspace.x[1] + acadoWorkspace.evGx[222]*acadoWorkspace.x[2] + acadoWorkspace.evGx[223]*acadoWorkspace.x[3] + acadoWorkspace.d[55];
-acadoVariables.x[60] += + acadoWorkspace.evGx[224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[227]*acadoWorkspace.x[3] + acadoWorkspace.d[56];
-acadoVariables.x[61] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.d[57];
-acadoVariables.x[62] += + acadoWorkspace.evGx[232]*acadoWorkspace.x[0] + acadoWorkspace.evGx[233]*acadoWorkspace.x[1] + acadoWorkspace.evGx[234]*acadoWorkspace.x[2] + acadoWorkspace.evGx[235]*acadoWorkspace.x[3] + acadoWorkspace.d[58];
-acadoVariables.x[63] += + acadoWorkspace.evGx[236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[239]*acadoWorkspace.x[3] + acadoWorkspace.d[59];
-acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.d[60];
-acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61];
-acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62];
-acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63];
-
-acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 4 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 4 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 8 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 8 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 8 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 16 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 20 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 16 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 28 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 16 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 32 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 16 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 16 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 40 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 20 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 44 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 20 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 20 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 52 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 20 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 56 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 20 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 64 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 68 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 76 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 80 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 88 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 92 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 100 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 104 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 28 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 112 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 116 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 124 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 128 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 136 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 140 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 32 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 148 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 152 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 160 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 164 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 172 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 184 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 188 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 196 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 200 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 208 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 212 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 40 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 220 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 224 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 232 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 236 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 244 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 248 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 256 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 260 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 44 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 268 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 272 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 280 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 284 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 292 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 296 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 304 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 308 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 316 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 320 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 328 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 332 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 340 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 344 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 352 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 356 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 52 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 364 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 368 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 376 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 380 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 388 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 392 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 400 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 404 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 412 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 416 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 56 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 424 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 428 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 436 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 440 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 448 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 452 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 460 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 464 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 472 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 476 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 484 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 488 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 496 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 500 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 508 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 512 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 520 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 524 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 532 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 536 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 64 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 64 ]) );
-}
-
-int acado_preparationStep(  )
-{
-int ret;
-
-ret = acado_modelSimulation();
-acado_evaluateObjective(  );
-acado_condensePrep(  );
-return ret;
-}
-
-int acado_feedbackStep(  )
-{
-int tmp;
-
-acado_condenseFdb(  );
-
-tmp = acado_solve( );
-
-acado_expand(  );
-return tmp;
-}
-
-int acado_initializeSolver(  )
-{
-int ret;
-
-/* This is a function which must be called once before any other function call! */
-
-
-ret = 0;
-
-memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
-return ret;
-}
-
-void acado_initializeNodesByForwardSimulation(  )
-{
-int index;
-for (index = 0; index < 16; ++index)
-{
-acadoWorkspace.state[0] = acadoVariables.x[index * 4];
-acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3];
-acadoWorkspace.state[24] = acadoVariables.u[index];
-acadoWorkspace.state[25] = acadoVariables.od[index * 2];
-acadoWorkspace.state[26] = acadoVariables.od[index * 2 + 1];
-
-acado_integrate(acadoWorkspace.state, index == 0, index);
-
-acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0];
-acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1];
-acadoVariables.x[index * 4 + 6] = acadoWorkspace.state[2];
-acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3];
-}
-}
-
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 16; ++index)
-{
-acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4];
-acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5];
-acadoVariables.x[index * 4 + 2] = acadoVariables.x[index * 4 + 6];
-acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7];
-}
-
-if (strategy == 1 && xEnd != 0)
-{
-acadoVariables.x[64] = xEnd[0];
-acadoVariables.x[65] = xEnd[1];
-acadoVariables.x[66] = xEnd[2];
-acadoVariables.x[67] = xEnd[3];
-}
-else if (strategy == 2) 
-{
-acadoWorkspace.state[0] = acadoVariables.x[64];
-acadoWorkspace.state[1] = acadoVariables.x[65];
-acadoWorkspace.state[2] = acadoVariables.x[66];
-acadoWorkspace.state[3] = acadoVariables.x[67];
-if (uEnd != 0)
-{
-acadoWorkspace.state[24] = uEnd[0];
-}
-else
-{
-acadoWorkspace.state[24] = acadoVariables.u[15];
-}
-acadoWorkspace.state[25] = acadoVariables.od[32];
-acadoWorkspace.state[26] = acadoVariables.od[33];
-
-acado_integrate(acadoWorkspace.state, 1, 15);
-
-acadoVariables.x[64] = acadoWorkspace.state[0];
-acadoVariables.x[65] = acadoWorkspace.state[1];
-acadoVariables.x[66] = acadoWorkspace.state[2];
-acadoVariables.x[67] = acadoWorkspace.state[3];
-}
-}
-
-void acado_shiftControls( real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 15; ++index)
-{
-acadoVariables.u[index] = acadoVariables.u[index + 1];
-}
-
-if (uEnd != 0)
-{
-acadoVariables.u[15] = uEnd[0];
-}
-}
-
-real_t acado_getKKT(  )
-{
-real_t kkt;
-
-int index;
-real_t prd;
-
-kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19];
-kkt = fabs( kkt );
-for (index = 0; index < 20; ++index)
-{
-prd = acadoWorkspace.y[index];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lb[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ub[index] * prd);
-}
-for (index = 0; index < 32; ++index)
-{
-prd = acadoWorkspace.y[index + 20];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lbA[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ubA[index] * prd);
-}
-return kkt;
-}
-
-real_t acado_getObjective(  )
-{
-real_t objVal;
-
-int lRun1;
-/** Row vector of size: 3 */
-real_t tmpDy[ 3 ];
-
-/** Row vector of size: 2 */
-real_t tmpDyN[ 2 ];
-
-for (lRun1 = 0; lRun1 < 16; ++lRun1)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[lRun1 * 3] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 3];
-acadoWorkspace.Dy[lRun1 * 3 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 3 + 1];
-acadoWorkspace.Dy[lRun1 * 3 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 3 + 2];
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[64];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[65];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[66];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[67];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[32];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[33];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
-objVal = 0.0000000000000000e+00;
-for (lRun1 = 0; lRun1 < 16; ++lRun1)
-{
-tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 3] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 6];
-tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 1] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 4] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 7];
-tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 3]*acadoVariables.W[lRun1 * 9 + 2] + acadoWorkspace.Dy[lRun1 * 3 + 1]*acadoVariables.W[lRun1 * 9 + 5] + acadoWorkspace.Dy[lRun1 * 3 + 2]*acadoVariables.W[lRun1 * 9 + 8];
-objVal += + acadoWorkspace.Dy[lRun1 * 3]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 3 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 3 + 2]*tmpDy[2];
-}
-
-tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[2];
-tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[3];
-objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1];
-
-objVal *= 0.5;
-return objVal;
-}
-
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
deleted file mode 100644
index 2ca3db3a2..000000000
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ /dev/null
@@ -1,32 +0,0 @@
-import os
-
-from cffi import FFI
-from common.ffi_wrapper import suffix
-
-mpc_dir = os.path.dirname(os.path.abspath(__file__))
-libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
-
-ffi = FFI()
-ffi.cdef("""
-typedef struct {
-    double x, y, psi, curvature, curvature_rate;
-} state_t;
-int N = 16;
-
-typedef struct {
-    double x[N+1];
-    double y[N+1];
-    double psi[N+1];
-    double curvature[N+1];
-    double curvature_rate[N];
-    double cost;
-} log_t;
-
-void init();
-void set_weights(double pathCost, double headingCost, double steerRateCost);
-int run_mpc(state_t * x0, log_t * solution,
-             double v_ego, double rotation_radius,
-             double target_y[N+1], double target_psi[N+1]);
-""")
-
-libmpc = ffi.dlopen(libmpc_fn)
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/.gitignore b/selfdrive/controls/lib/lateral_mpc_lib/.gitignore
new file mode 100644
index 000000000..aff2eb082
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/.gitignore
@@ -0,0 +1,2 @@
+acados_ocp_lat.json
+c_generated_code/
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/SConscript b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
new file mode 100644
index 000000000..4ebb5cd4e
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/SConscript
@@ -0,0 +1,58 @@
+Import('env', 'arch')
+
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/lat_model/lat_expl_ode_fun.c',
+  f'{gen}/lat_model/lat_expl_vde_forw.c',
+]
+
+casadi_cost_y = [
+  f'{gen}/lat_cost/lat_cost_y_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_hess.c',
+]
+
+casadi_cost_e = [
+  f'{gen}/lat_cost/lat_cost_y_e_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_e_hess.c',
+]
+
+casadi_cost_0 = [
+  f'{gen}/lat_cost/lat_cost_y_0_fun.c',
+  f'{gen}/lat_cost/lat_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/lat_cost/lat_cost_y_0_hess.c',
+]
+
+build_files = [f'{gen}/acados_solver_lat.c'] + casadi_model + casadi_cost_y + casadi_cost_e + casadi_cost_0
+
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
+
+  f'{gen}/main_lat.c',
+  f'{gen}/acados_solver_lat.h',
+
+  f'{gen}/lat_model/lat_expl_vde_adj.c',
+
+  f'{gen}/lat_model/lat_model.h',
+  f'{gen}/lat_cost/lat_cost_y_fun.h',
+  f'{gen}/lat_cost/lat_cost_y_e_fun.h',
+  f'{gen}/lat_cost/lat_cost_y_0_fun.h',
+] + build_files
+
+lenv = env.Clone()
+lenv.Clean(generated_files, Dir(gen))
+
+lenv.Command(generated_files,
+             ["lat_mpc.py"],
+             f"cd {Dir('.').abspath} && python lat_mpc.py")
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
+                   build_files,
+                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
diff --git a/selfdrive/controls/lib/lateral_mpc/__init__.py b/selfdrive/controls/lib/lateral_mpc_lib/__init__.py
similarity index 100%
rename from selfdrive/controls/lib/lateral_mpc/__init__.py
rename to selfdrive/controls/lib/lateral_mpc_lib/__init__.py
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
new file mode 100755
index 000000000..6b0290863
--- /dev/null
+++ b/selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
@@ -0,0 +1,158 @@
+#!/usr/bin/env python3
+import os
+import numpy as np
+
+from casadi import SX, vertcat, sin, cos
+from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
+from selfdrive.controls.lib.drive_helpers import T_IDXS
+from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+
+
+LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
+JSON_FILE = "acados_ocp_lat.json"
+X_DIM = 6
+
+def gen_lat_model():
+  model = AcadosModel()
+  model.name = 'lat'
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  y_ego = SX.sym('y_ego')
+  psi_ego = SX.sym('psi_ego')
+  curv_ego = SX.sym('curv_ego')
+  v_ego = SX.sym('v_ego')
+  rotation_radius = SX.sym('rotation_radius')
+  model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius)
+
+  # controls
+  curv_rate = SX.sym('curv_rate')
+  model.u = vertcat(curv_rate)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  y_ego_dot = SX.sym('y_ego_dot')
+  psi_ego_dot = SX.sym('psi_ego_dot')
+  curv_ego_dot = SX.sym('curv_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  rotation_radius_dot = SX.sym('rotation_radius_dot')
+  model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot,
+                       v_ego_dot, rotation_radius_dot)
+
+  # dynamics model
+  f_expl = vertcat(v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego),
+                   v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego),
+                   v_ego * curv_ego,
+                   curv_rate,
+                   0.0,
+                   0.0)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_lat_mpc_solver():
+  ocp = AcadosOcp()
+  ocp.model = gen_lat_model()
+
+  Tf = np.array(T_IDXS)[N]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  Q = np.diag([0.0, 0.0])
+  QR = np.diag([0.0, 0.0, 0.0])
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  y_ego, psi_ego = ocp.model.x[1], ocp.model.x[2]
+  curv_rate = ocp.model.u[0]
+  v_ego = ocp.model.x[4]
+
+
+  ocp.cost.yref = np.zeros((3, ))
+  ocp.cost.yref_e = np.zeros((2, ))
+  # TODO hacky weights to keep behavior the same
+  ocp.model.cost_y_expr = vertcat(y_ego,
+                                  ((v_ego +5.0) * psi_ego),
+                                  ((v_ego +5.0) * 4 * curv_rate))
+  ocp.model.cost_y_expr_e = vertcat(y_ego,
+                                    ((v_ego +5.0) * psi_ego))
+
+  # set constraints
+  ocp.constraints.constr_type = 'BGH'
+  ocp.constraints.idxbx = np.array([2,3])
+  ocp.constraints.ubx = np.array([np.radians(90), np.radians(50)])
+  ocp.constraints.lbx = np.array([-np.radians(90), -np.radians(50)])
+  x0 = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+  ocp.constraints.x0 = x0
+
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+  ocp.solver_options.qp_solver_iter_max = 1
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = np.array(T_IDXS)[:N+1]
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LateralMpc():
+  def __init__(self, x0=np.zeros(X_DIM)):
+    self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
+    self.reset(x0)
+
+  def reset(self, x0=np.zeros(X_DIM)):
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N, 1))
+    self.yref = np.zeros((N+1, 3))
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
+    self.solver.cost_set(N, "yref", self.yref[N][:2])
+    W = np.eye(3)
+    self.Ws = np.tile(W[None], reps=(N,1,1))
+
+    # Somehow needed for stable init
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+    self.solver.constraints_set(0, "lbx", x0)
+    self.solver.constraints_set(0, "ubx", x0)
+    self.solver.solve()
+    self.solution_status = 0
+    self.cost = 0
+
+  def set_weights(self, path_weight, heading_weight, steer_rate_weight):
+    self.Ws[:,0,0] = path_weight
+    self.Ws[:,1,1] = heading_weight
+    self.Ws[:,2,2] = steer_rate_weight
+    self.solver.cost_set_slice(0, N, 'W', self.Ws, api='old')
+    #TODO hacky weights to keep behavior the same
+    self.solver.cost_set(N, 'W', (3/20.)*self.Ws[0,:2,:2])
+
+  def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
+    x0_cp = np.copy(x0)
+    self.solver.constraints_set(0, "lbx", x0_cp)
+    self.solver.constraints_set(0, "ubx", x0_cp)
+    self.yref[:,0] = y_pts
+    self.yref[:,1] = heading_pts*(v_ego+5.0)
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
+    self.solver.cost_set(N, "yref", self.yref[N][:2])
+
+    self.solution_status = self.solver.solve()
+    self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
+    self.solver.fill_in_slice(0, N, 'u', self.u_sol)
+    self.cost = self.solver.get_cost()
+
+
+if __name__ == "__main__":
+  ocp = gen_lat_mpc_solver()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 0dab9503b..27041e1f0 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -3,7 +3,7 @@ import numpy as np
 from common.realtime import sec_since_boot, DT_MDL
 from common.numpy_fast import interp
 from selfdrive.swaglog import cloudlog
-from selfdrive.controls.lib.lateral_mpc import libmpc_py
+from selfdrive.controls.lib.lateral_mpc_lib.lat_mpc import LateralMpc
 from selfdrive.controls.lib.drive_helpers import CONTROL_N, MPC_COST_LAT, LAT_MPC_N, CAR_ROTATION_RADIUS
 from selfdrive.controls.lib.lane_planner import LanePlanner, TRAJECTORY_SIZE
 from selfdrive.config import Conversions as CV
@@ -46,7 +46,6 @@ class LateralPlanner():
     self.last_cloudlog_t = 0
     self.steer_rate_cost = CP.steerRateCost
 
-    self.setup_mpc()
     self.solution_invalid_cnt = 0
     self.lane_change_state = LaneChangeState.off
     self.lane_change_direction = LaneChangeDirection.none
@@ -62,17 +61,12 @@ class LateralPlanner():
     self.t_idxs = np.arange(TRAJECTORY_SIZE)
     self.y_pts = np.zeros(TRAJECTORY_SIZE)
 
-  def setup_mpc(self):
-    self.libmpc = libmpc_py.libmpc
-    self.libmpc.init()
-
-    self.mpc_solution = libmpc_py.ffi.new("log_t *")
-    self.cur_state = libmpc_py.ffi.new("state_t *")
-    self.cur_state[0].x = 0.0
-    self.cur_state[0].y = 0.0
-    self.cur_state[0].psi = 0.0
-    self.cur_state[0].curvature = 0.0
+    self.lat_mpc = LateralMpc()
+    self.reset_mpc(np.zeros(6))
 
+  def reset_mpc(self, x0=np.zeros(6)):
+    self.x0 = x0
+    self.lat_mpc.reset(x0=self.x0)
     self.desired_curvature = 0.0
     self.safe_desired_curvature = 0.0
     self.desired_curvature_rate = 0.0
@@ -89,7 +83,7 @@ class LateralPlanner():
       self.path_xyz = np.column_stack([md.position.x, md.position.y, md.position.z])
       self.t_idxs = np.array(md.position.t)
       self.plan_yaw = list(md.orientation.z)
-    if len(md.orientation.xStd) == TRAJECTORY_SIZE:
+    if len(md.position.xStd) == TRAJECTORY_SIZE:
       self.path_xyz_stds = np.column_stack([md.position.xStd, md.position.yStd, md.position.zStd])
 
     # Lane change logic
@@ -171,45 +165,40 @@ class LateralPlanner():
       self.LP.rll_prob *= self.lane_change_ll_prob
     if self.use_lanelines:
       d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
-      self.libmpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
+      self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
     else:
       d_path_xyz = self.path_xyz
       path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 5.0) * MPC_COST_LAT.PATH
       # Heading cost is useful at low speed, otherwise end of plan can be off-heading
       heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
-      self.libmpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
+      self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
     y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
     heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
     self.y_pts = y_pts
 
     assert len(y_pts) == LAT_MPC_N + 1
     assert len(heading_pts) == LAT_MPC_N + 1
-    # for now CAR_ROTATION_RADIUS is disabled
-    # to use it, enable it in the MPC
-    assert abs(CAR_ROTATION_RADIUS) < 1e-3
-    self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        float(v_ego),
-                        CAR_ROTATION_RADIUS,
-                        list(y_pts),
-                        list(heading_pts))
+    self.x0[4] = v_ego
+    self.lat_mpc.run(self.x0,
+                     v_ego,
+                     CAR_ROTATION_RADIUS,
+                     y_pts,
+                     heading_pts)
     # init state for next
-    self.cur_state.x = 0.0
-    self.cur_state.y = 0.0
-    self.cur_state.psi = 0.0
-    self.cur_state.curvature = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.mpc_solution.curvature)
+    self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:,3])
+
 
     #  Check for infeasable MPC solution
-    mpc_nans = any(math.isnan(x) for x in self.mpc_solution.curvature)
+    mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:,3])
     t = sec_since_boot()
-    if mpc_nans:
-      self.libmpc.init()
-      self.cur_state.curvature = measured_curvature
-
+    if mpc_nans or self.lat_mpc.solution_status != 0:
+      self.reset_mpc()
+      self.x0[3] = measured_curvature
       if t > self.last_cloudlog_t + 5.0:
         self.last_cloudlog_t = t
         cloudlog.warning("Lateral mpc - nan: True")
 
-    if self.mpc_solution[0].cost > 20000. or mpc_nans:   # TODO: find a better way to detect when MPC did not converge
+    if self.lat_mpc.cost > 20000. or mpc_nans:
       self.solution_invalid_cnt += 1
     else:
       self.solution_invalid_cnt = 0
@@ -220,9 +209,9 @@ class LateralPlanner():
     plan_send.valid = sm.all_alive_and_valid(service_list=['carState', 'controlsState', 'modelV2'])
     plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
     plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
-    plan_send.lateralPlan.psis = [float(x) for x in self.mpc_solution.psi[0:CONTROL_N]]
-    plan_send.lateralPlan.curvatures = [float(x) for x in self.mpc_solution.curvature[0:CONTROL_N]]
-    plan_send.lateralPlan.curvatureRates = [float(x) for x in self.mpc_solution.curvature_rate[0:CONTROL_N-1]] +[0.0]
+    plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
+    plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N,3]]
+    plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N-1]] +[0.0]
     plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
     plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
     plan_send.lateralPlan.dProb = float(self.LP.d_prob)
@@ -230,6 +219,7 @@ class LateralPlanner():
     plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
 
     plan_send.lateralPlan.desire = self.desire
+    plan_send.lateralPlan.useLaneLines = self.use_lanelines
     plan_send.lateralPlan.laneChangeState = self.lane_change_state
     plan_send.lateralPlan.laneChangeDirection = self.lane_change_direction
 
diff --git a/selfdrive/controls/lib/lead_mpc.py b/selfdrive/controls/lib/lead_mpc.py
deleted file mode 100644
index ee8ea98e7..000000000
--- a/selfdrive/controls/lib/lead_mpc.py
+++ /dev/null
@@ -1,111 +0,0 @@
-import math
-import numpy as np
-from common.numpy_fast import interp
-from common.realtime import sec_since_boot
-from selfdrive.modeld.constants import T_IDXS
-from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
-from selfdrive.controls.lib.lead_mpc_lib import libmpc_py
-from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG, CONTROL_N
-from selfdrive.swaglog import cloudlog
-
-MPC_T = list(np.arange(0,1.,.2)) + list(np.arange(1.,10.6,.6))
-
-
-class LeadMpc():
-  def __init__(self, mpc_id):
-    self.lead_id = mpc_id
-
-    self.reset_mpc()
-    self.prev_lead_status = False
-    self.prev_lead_x = 0.0
-    self.new_lead = False
-
-    self.last_cloudlog_t = 0.0
-    self.n_its = 0
-    self.duration = 0
-    self.status = False
-
-    self.v_solution = np.zeros(CONTROL_N)
-    self.a_solution = np.zeros(CONTROL_N)
-    self.j_solution = np.zeros(CONTROL_N)
-
-  def reset_mpc(self):
-    ffi, self.libmpc = libmpc_py.get_libmpc(self.lead_id)
-    self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
-                     MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-
-    self.mpc_solution = ffi.new("log_t *")
-    self.cur_state = ffi.new("state_t *")
-    self.cur_state[0].v_ego = 0
-    self.cur_state[0].a_ego = 0
-    self.a_lead_tau = _LEAD_ACCEL_TAU
-
-  def set_cur_state(self, v, a):
-    v_safe = max(v, 1e-3)
-    a_safe = a
-    self.cur_state[0].v_ego = v_safe
-    self.cur_state[0].a_ego = a_safe
-
-  def update(self, CS, radarstate, v_cruise):
-    v_ego = CS.vEgo
-    if self.lead_id == 0:
-      lead = radarstate.leadOne
-    else:
-      lead = radarstate.leadTwo
-    self.status = lead.status
-
-    # Setup current mpc state
-    self.cur_state[0].x_ego = 0.0
-
-    if lead is not None and lead.status:
-      x_lead = lead.dRel
-      v_lead = max(0.0, lead.vLead)
-      a_lead = lead.aLeadK
-
-      if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
-        v_lead = 0.0
-        a_lead = 0.0
-
-      self.a_lead_tau = lead.aLeadTau
-      self.new_lead = False
-      if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
-        self.libmpc.init_with_simulation(v_ego, x_lead, v_lead, a_lead, self.a_lead_tau)
-        self.new_lead = True
-
-      self.prev_lead_status = True
-      self.prev_lead_x = x_lead
-      self.cur_state[0].x_l = x_lead
-      self.cur_state[0].v_l = v_lead
-    else:
-      self.prev_lead_status = False
-      # Fake a fast lead car, so mpc keeps running
-      self.cur_state[0].x_l = 50.0
-      self.cur_state[0].v_l = v_ego + 10.0
-      a_lead = 0.0
-      self.a_lead_tau = _LEAD_ACCEL_TAU
-
-    # Calculate mpc
-    t = sec_since_boot()
-    self.n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
-    self.v_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.v_ego)
-    self.a_solution = interp(T_IDXS[:CONTROL_N], MPC_T, self.mpc_solution.a_ego)
-    self.j_solution = interp(T_IDXS[:CONTROL_N], MPC_T[:-1], self.mpc_solution.j_ego)
-    self.duration = int((sec_since_boot() - t) * 1e9)
-
-    # Reset if NaN or goes through lead car
-    crashing = any(lead - ego < -50 for (lead, ego) in zip(self.mpc_solution[0].x_l, self.mpc_solution[0].x_ego))
-    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
-    backwards = min(self.mpc_solution[0].v_ego) < -0.15
-
-    if ((backwards or crashing) and self.prev_lead_status) or nans:
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
-                          self.lead_id, backwards, crashing, nans))
-
-      self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
-                       MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-      self.cur_state[0].v_ego = v_ego
-      self.cur_state[0].a_ego = 0.0
-      self.a_mpc = CS.aEgo
-      self.prev_lead_status = False
diff --git a/selfdrive/controls/lib/lead_mpc_lib/.gitignore b/selfdrive/controls/lib/lead_mpc_lib/.gitignore
deleted file mode 100644
index f61a939cd..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/.gitignore
+++ /dev/null
@@ -1,2 +0,0 @@
-generator
-lib_qp/
diff --git a/selfdrive/controls/lib/lead_mpc_lib/SConscript b/selfdrive/controls/lib/lead_mpc_lib/SConscript
deleted file mode 100644
index 94deb5d6f..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/SConscript
+++ /dev/null
@@ -1,48 +0,0 @@
-Import('env', 'arch')
-
-
-cpp_path = [
-    "#phonelibs/acado/include",
-    "#phonelibs/acado/include/acado",
-    "#phonelibs/qpoases/INCLUDE",
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
-    "#phonelibs/qpoases/SRC/",
-    "#phonelibs/qpoases",
-    "lib_mpc_export",
-]
-
-generated_c = [
-    'lib_mpc_export/acado_auxiliary_functions.c',
-    'lib_mpc_export/acado_qpoases_interface.cpp',
-    'lib_mpc_export/acado_integrator.c',
-    'lib_mpc_export/acado_solver.c',
-]
-
-generated_h = [
-    'lib_mpc_export/acado_common.h',
-    'lib_mpc_export/acado_auxiliary_functions.h',
-    'lib_mpc_export/acado_qpoases_interface.hpp',
-]
-
-
-interface_dir = Dir('lib_mpc_export')
-
-SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
-
-if GetOption('mpc_generate'):
-    generator_cpp = File('generator.cpp')
-
-    acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                  File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
-
-    generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
-                            CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
-
-    cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
-    env.Command(generated_c + generated_h, generator, cmd)
-
-
-mpc_files = ["longitudinal_mpc.c"] + generated_c
-env.SharedLibrary('mpc0', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
-env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
diff --git a/selfdrive/controls/lib/lead_mpc_lib/generator.cpp b/selfdrive/controls/lib/lead_mpc_lib/generator.cpp
deleted file mode 100644
index 9941846a5..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/generator.cpp
+++ /dev/null
@@ -1,97 +0,0 @@
-#include <acado_code_generation.hpp>
-
-const int controlHorizon = 50;
-
-using namespace std;
-
-#define G 9.81
-#define TR 1.8
-
-#define RW(v_ego, v_l) (v_ego * TR - (v_l - v_ego) * TR + v_ego*v_ego/(2*G) - v_l*v_l / (2*G))
-#define NORM_RW_ERROR(v_ego, v_l, p) ((RW(v_ego, v_l) + 4.0 - p)/(sqrt(v_ego + 0.5) + 0.1))
-
-int main( )
-{
-  USING_NAMESPACE_ACADO
-
-
-  DifferentialEquation f;
-
-  DifferentialState x_ego, v_ego, a_ego;
-  OnlineData x_l, v_l;
-
-  Control j_ego;
-
-  auto desired = 4.0 + RW(v_ego, v_l);
-  auto d_l = x_l - x_ego;
-
-  // Equations of motion
-  f << dot(x_ego) == v_ego;
-  f << dot(v_ego) == a_ego;
-  f << dot(a_ego) == j_ego;
-
-  // Running cost
-  Function h;
-  h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
-  h << (d_l - desired) / (0.05 * v_ego + 0.5);
-  h << a_ego * (0.1 * v_ego + 1.0);
-  h << j_ego * (0.1 * v_ego + 1.0);
-
-  // Weights are defined in mpc.
-  BMatrix Q(4,4); Q.setAll(true);
-
-  // Terminal cost
-  Function hN;
-  hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - 1;
-  hN << (d_l - desired) / (0.05 * v_ego + 0.5);
-  hN << a_ego * (0.1 * v_ego + 1.0);
-
-  // Weights are defined in mpc.
-  BMatrix QN(3,3); QN.setAll(true);
-
-  // Non uniform time grid
-  // First 5 timesteps are 0.2, after that it's 0.6
-  DMatrix numSteps(20, 1);
-  for (int i = 0; i < 5; i++){
-    numSteps(i) = 1;
-  }
-  for (int i = 5; i < 20; i++){
-    numSteps(i) = 3;
-  }
-
-  // Setup Optimal Control Problem
-  const double tStart = 0.0;
-  const double tEnd   = 10.0;
-
-  OCP ocp( tStart, tEnd, numSteps);
-  ocp.subjectTo(f);
-
-  ocp.minimizeLSQ(Q, h);
-  ocp.minimizeLSQEndTerm(QN, hN);
-
-  ocp.subjectTo( -0.1 <= v_ego);
-  ocp.setNOD(2);
-
-  OCPexport mpc(ocp);
-  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
-  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
-  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, controlHorizon);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
-  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
-
-  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
-  mpc.set( QP_SOLVER, QP_QPOASES );
-  mpc.set( HOTSTART_QP, YES );
-  mpc.set( GENERATE_TEST_FILE, NO);
-  mpc.set( GENERATE_MAKE_FILE, NO );
-  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
-  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
-
-  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
-    exit( EXIT_FAILURE );
-
-  mpc.printDimensionsQP( );
-
-  return EXIT_SUCCESS;
-}
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c
deleted file mode 100644
index 6f0bb705c..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_auxiliary_functions.h"
-
-#include <stdio.h>
-
-real_t* acado_getVariablesX( )
-{
-	return acadoVariables.x;
-}
-
-real_t* acado_getVariablesU( )
-{
-	return acadoVariables.u;
-}
-
-#if ACADO_NY > 0
-real_t* acado_getVariablesY( )
-{
-	return acadoVariables.y;
-}
-#endif
-
-#if ACADO_NYN > 0
-real_t* acado_getVariablesYN( )
-{
-	return acadoVariables.yN;
-}
-#endif
-
-real_t* acado_getVariablesX0( )
-{
-#if ACADO_INITIAL_VALUE_FIXED
-	return acadoVariables.x0;
-#else
-	return 0;
-#endif
-}
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( )
-{
-	int i, j;
-	printf("\nDifferential variables:\n[\n");
-	for (i = 0; i < ACADO_N + 1; ++i)
-	{
-		for (j = 0; j < ACADO_NX; ++j)
-			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print control variables. */
-void acado_printControlVariables( )
-{
-	int i, j;
-	printf("\nControl variables:\n[\n");
-	for (i = 0; i < ACADO_N; ++i)
-	{
-		for (j = 0; j < ACADO_NU; ++j)
-			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( )
-{
-	printf(
-		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
-		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
-		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
-	);
-	
-	printf(
-		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
-		"supervision of Moritz Diehl. All rights reserved.\n\n"
-		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
-		"General Public License 3 in the hope that it will be useful,\n"
-		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
-		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
-		"GNU Lesser General Public License for more details.\n\n"
-	);
-}
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-void acado_tic( acado_timer* t )
-{
-	QueryPerformanceFrequency(&t->freq);
-	QueryPerformanceCounter(&t->tic);
-}
-
-real_t acado_toc( acado_timer* t )
-{
-	QueryPerformanceCounter(&t->toc);
-	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
-}
-
-
-#elif (defined __APPLE__)
-
-void acado_tic( acado_timer* t )
-{
-    /* read current clock cycles */
-    t->tic = mach_absolute_time();
-}
-
-real_t acado_toc( acado_timer* t )
-{
-
-    uint64_t duration; /* elapsed time in clock cycles*/
-    
-    t->toc = mach_absolute_time();
-    duration = t->toc - t->tic;
-    
-    /*conversion from clock cycles to nanoseconds*/
-    mach_timebase_info(&(t->tinfo));
-    duration *= t->tinfo.numer;
-    duration /= t->tinfo.denom;
-
-    return (real_t)duration / 1e9;
-}
-
-#else
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	gettimeofday(&t->tic, 0);
-}
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timeval temp;
-	
-	gettimeofday(&t->toc, 0);
-    
-	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
-}
-
-#else
-/* ANSI */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	clock_gettime(CLOCK_MONOTONIC, &t->tic);
-}
-
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timespec temp;
-    
-	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
-    
-	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
-}
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || _WIN64) */
-
-#endif
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h
deleted file mode 100644
index b1be0a661..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_AUXILIARY_FUNCTIONS_H
-#define ACADO_AUXILIARY_FUNCTIONS_H
-
-#include "acado_common.h"
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** Get pointer to the matrix with differential variables. */
-real_t* acado_getVariablesX( );
-
-/** Get pointer to the matrix with control variables. */
-real_t* acado_getVariablesU( );
-
-#if ACADO_NY > 0
-/** Get pointer to the matrix with references/measurements. */
-real_t* acado_getVariablesY( );
-#endif
-
-#if ACADO_NYN > 0
-/** Get pointer to the vector with references/measurement on the last node. */
-real_t* acado_getVariablesYN( );
-#endif
-
-/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
-real_t* acado_getVariablesX0( );
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( );
-
-/** Print control variables. */
-void acado_printControlVariables( );
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( );
-
-/*
- * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
- * providing us with the following timing routines.
- */
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-/* Use Windows QueryPerformanceCounter for timing. */
-#include <Windows.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	LARGE_INTEGER tic;
-	LARGE_INTEGER toc;
-	LARGE_INTEGER freq;
-} acado_timer;
-
-
-#elif (defined __APPLE__)
-
-#include "unistd.h"
-#include <mach/mach_time.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	uint64_t tic;
-	uint64_t toc;
-	mach_timebase_info_data_t tinfo;
-} acado_timer;
-
-#else
-
-/* Use POSIX clock_gettime() for timing on non-Windows machines. */
-#include <time.h>
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode of operation. */
-
-#include <sys/stat.h>
-#include <sys/time.h>
-
-typedef struct acado_timer_
-{
-	struct timeval tic;
-	struct timeval toc;
-} acado_timer;
-
-#else
-/* ANSI C */
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	struct timespec tic;
-	struct timespec toc;
-} acado_timer;
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || defined _WIN64) */
-
-/** A function for measurement of the current time. */
-void acado_tic( acado_timer* t );
-
-/** A function which returns the elapsed time. */
-real_t acado_toc( acado_timer* t );
-
-#endif
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_AUXILIARY_FUNCTIONS_H */
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_common.h
deleted file mode 100644
index c0eb83bce..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_common.h
+++ /dev/null
@@ -1,354 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_COMMON_H
-#define ACADO_COMMON_H
-
-#include <math.h>
-#include <string.h>
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** \defgroup ACADO ACADO CGT generated module. */
-/** @{ */
-
-/** qpOASES QP solver indicator. */
-#define ACADO_QPOASES  0
-#define ACADO_QPOASES3 1
-/** FORCES QP solver indicator.*/
-#define ACADO_FORCES   2
-/** qpDUNES QP solver indicator.*/
-#define ACADO_QPDUNES  3
-/** HPMPC QP solver indicator. */
-#define ACADO_HPMPC    4
-#define ACADO_GENERIC    5
-
-/** Indicator for determining the QP solver used by the ACADO solver code. */
-#define ACADO_QP_SOLVER ACADO_QPOASES
-
-#include "acado_qpoases_interface.hpp"
-
-
-/*
- * Common definitions
- */
-/** User defined block based condensing. */
-#define ACADO_BLOCK_CONDENSING 0
-/** Compute covariance matrix of the last state estimate. */
-#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
-/** Flag indicating whether constraint values are hard-coded or not. */
-#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
-/** Indicator for fixed initial state. */
-#define ACADO_INITIAL_STATE_FIXED 1
-/** Number of control/estimation intervals. */
-#define ACADO_N 20
-/** Number of online data values. */
-#define ACADO_NOD 2
-/** Number of path constraints. */
-#define ACADO_NPAC 0
-/** Number of control variables. */
-#define ACADO_NU 1
-/** Number of differential variables. */
-#define ACADO_NX 3
-/** Number of algebraic variables. */
-#define ACADO_NXA 0
-/** Number of differential derivative variables. */
-#define ACADO_NXD 0
-/** Number of references/measurements per node on the first N nodes. */
-#define ACADO_NY 4
-/** Number of references/measurements on the last (N + 1)st node. */
-#define ACADO_NYN 3
-/** Total number of QP optimization variables. */
-#define ACADO_QP_NV 23
-/** Number of Runge-Kutta stages per integration step. */
-#define ACADO_RK_NSTAGES 4
-/** Providing interface for arrival cost. */
-#define ACADO_USE_ARRIVAL_COST 0
-/** Indicator for usage of non-hard-coded linear terms in the objective. */
-#define ACADO_USE_LINEAR_TERMS 0
-/** Indicator for type of fixed weighting matrices. */
-#define ACADO_WEIGHTING_MATRICES_TYPE 2
-
-
-/*
- * Globally used structure definitions
- */
-
-/** The structure containing the user data.
- * 
- *  Via this structure the user "communicates" with the solver code.
- */
-typedef struct ACADOvariables_
-{
-int dummy;
-/** Matrix of size: 21 x 3 (row major format)
- * 
- *  Matrix containing 21 differential variable vectors.
- */
-real_t x[ 63 ];
-
-/** Column vector of size: 20
- * 
- *  Matrix containing 20 control variable vectors.
- */
-real_t u[ 20 ];
-
-/** Matrix of size: 21 x 2 (row major format)
- * 
- *  Matrix containing 21 online data vectors.
- */
-real_t od[ 42 ];
-
-/** Column vector of size: 80
- * 
- *  Matrix containing 20 reference/measurement vectors of size 4 for first 20 nodes.
- */
-real_t y[ 80 ];
-
-/** Column vector of size: 3
- * 
- *  Reference/measurement vector for the 21. node.
- */
-real_t yN[ 3 ];
-
-/** Matrix of size: 80 x 4 (row major format) */
-real_t W[ 320 ];
-
-/** Matrix of size: 3 x 3 (row major format) */
-real_t WN[ 9 ];
-
-/** Column vector of size: 3
- * 
- *  Current state feedback vector.
- */
-real_t x0[ 3 ];
-
-
-} ACADOvariables;
-
-/** Private workspace used by the auto-generated code.
- * 
- *  Data members of this structure are private to the solver.
- *  In other words, the user code should not modify values of this 
- *  structure. 
- */
-typedef struct ACADOworkspace_
-{
-real_t rk_ttt;
-
-/** Row vector of size: 18 */
-real_t rk_xxx[ 18 ];
-
-/** Matrix of size: 4 x 15 (row major format) */
-real_t rk_kkk[ 60 ];
-
-/** Row vector of size: 18 */
-real_t state[ 18 ];
-
-/** Column vector of size: 60 */
-real_t d[ 60 ];
-
-/** Column vector of size: 80 */
-real_t Dy[ 80 ];
-
-/** Column vector of size: 3 */
-real_t DyN[ 3 ];
-
-/** Matrix of size: 60 x 3 (row major format) */
-real_t evGx[ 180 ];
-
-/** Column vector of size: 60 */
-real_t evGu[ 60 ];
-
-/** Column vector of size: 13 */
-real_t objAuxVar[ 13 ];
-
-/** Row vector of size: 6 */
-real_t objValueIn[ 6 ];
-
-/** Row vector of size: 20 */
-real_t objValueOut[ 20 ];
-
-/** Matrix of size: 60 x 3 (row major format) */
-real_t Q1[ 180 ];
-
-/** Matrix of size: 60 x 4 (row major format) */
-real_t Q2[ 240 ];
-
-/** Column vector of size: 20 */
-real_t R1[ 20 ];
-
-/** Matrix of size: 20 x 4 (row major format) */
-real_t R2[ 80 ];
-
-/** Column vector of size: 60 */
-real_t S1[ 60 ];
-
-/** Matrix of size: 3 x 3 (row major format) */
-real_t QN1[ 9 ];
-
-/** Matrix of size: 3 x 3 (row major format) */
-real_t QN2[ 9 ];
-
-/** Column vector of size: 3 */
-real_t Dx0[ 3 ];
-
-/** Matrix of size: 3 x 3 (row major format) */
-real_t T[ 9 ];
-
-/** Column vector of size: 630 */
-real_t E[ 630 ];
-
-/** Column vector of size: 630 */
-real_t QE[ 630 ];
-
-/** Matrix of size: 60 x 3 (row major format) */
-real_t QGx[ 180 ];
-
-/** Column vector of size: 60 */
-real_t Qd[ 60 ];
-
-/** Column vector of size: 63 */
-real_t QDy[ 63 ];
-
-/** Matrix of size: 20 x 3 (row major format) */
-real_t H10[ 60 ];
-
-/** Matrix of size: 23 x 23 (row major format) */
-real_t H[ 529 ];
-
-/** Matrix of size: 20 x 23 (row major format) */
-real_t A[ 460 ];
-
-/** Column vector of size: 23 */
-real_t g[ 23 ];
-
-/** Column vector of size: 23 */
-real_t lb[ 23 ];
-
-/** Column vector of size: 23 */
-real_t ub[ 23 ];
-
-/** Column vector of size: 20 */
-real_t lbA[ 20 ];
-
-/** Column vector of size: 20 */
-real_t ubA[ 20 ];
-
-/** Column vector of size: 23 */
-real_t x[ 23 ];
-
-/** Column vector of size: 43 */
-real_t y[ 43 ];
-
-
-} ACADOworkspace;
-
-/* 
- * Forward function declarations. 
- */
-
-
-/** Performs the integration and sensitivity propagation for one shooting interval.
- *
- *  \param rk_eta Working array to pass the input values and return the results.
- *  \param resetIntegrator The internal memory of the integrator can be reset.
- *  \param rk_index Number of the shooting interval.
- *
- *  \return Status code of the integrator.
- */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
-
-/** Export of an ACADO symbolic function.
- *
- *  \param in Input to the exported function.
- *  \param out Output of the exported function.
- */
-void acado_rhs_forw(const real_t* in, real_t* out);
-
-/** Preparation step of the RTI scheme.
- *
- *  \return Status of the integration module. =0: OK, otherwise the error code.
- */
-int acado_preparationStep(  );
-
-/** Feedback/estimation step of the RTI scheme.
- *
- *  \return Status code of the qpOASES QP solver.
- */
-int acado_feedbackStep(  );
-
-/** Solver initialization. Must be called once before any other function call.
- *
- *  \return =0: OK, otherwise an error code of a QP solver.
- */
-int acado_initializeSolver(  );
-
-/** Initialize shooting nodes by a forward simulation starting from the first node.
- */
-void acado_initializeNodesByForwardSimulation(  );
-
-/** Shift differential variables vector by one interval.
- *
- *  \param strategy Shifting strategy: 1. Initialize node 21 with xEnd. 2. Initialize node 21 by forward simulation.
- *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
-
-/** Shift controls vector by one interval.
- *
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftControls( real_t* const uEnd );
-
-/** Get the KKT tolerance of the current iterate.
- *
- *  \return The KKT tolerance value.
- */
-real_t acado_getKKT(  );
-
-/** Calculate the objective value.
- *
- *  \return Value of the objective function.
- */
-real_t acado_getObjective(  );
-
-
-/* 
- * Extern declarations. 
- */
-
-extern ACADOworkspace acadoWorkspace;
-extern ACADOvariables acadoVariables;
-
-/** @} */
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_COMMON_H */
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_integrator.c
deleted file mode 100644
index a0bac9681..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_integrator.c
+++ /dev/null
@@ -1,159 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-void acado_rhs_forw(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 15;
-
-/* Compute outputs: */
-out[0] = xd[1];
-out[1] = xd[2];
-out[2] = u[0];
-out[3] = xd[6];
-out[4] = xd[7];
-out[5] = xd[8];
-out[6] = xd[9];
-out[7] = xd[10];
-out[8] = xd[11];
-out[9] = (real_t)(0.0000000000000000e+00);
-out[10] = (real_t)(0.0000000000000000e+00);
-out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = xd[13];
-out[13] = xd[14];
-out[14] = (real_t)(1.0000000000000000e+00);
-}
-
-/* Fixed step size:0.2 */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
-{
-int error;
-
-int run1;
-int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
-int numInts = numSteps[rk_index];
-acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
-rk_eta[3] = 1.0000000000000000e+00;
-rk_eta[4] = 0.0000000000000000e+00;
-rk_eta[5] = 0.0000000000000000e+00;
-rk_eta[6] = 0.0000000000000000e+00;
-rk_eta[7] = 1.0000000000000000e+00;
-rk_eta[8] = 0.0000000000000000e+00;
-rk_eta[9] = 0.0000000000000000e+00;
-rk_eta[10] = 0.0000000000000000e+00;
-rk_eta[11] = 1.0000000000000000e+00;
-rk_eta[12] = 0.0000000000000000e+00;
-rk_eta[13] = 0.0000000000000000e+00;
-rk_eta[14] = 0.0000000000000000e+00;
-acadoWorkspace.rk_xxx[15] = rk_eta[15];
-acadoWorkspace.rk_xxx[16] = rk_eta[16];
-acadoWorkspace.rk_xxx[17] = rk_eta[17];
-
-for (run1 = 0; run1 < 1; ++run1)
-{
-for(run1 = 0; run1 < numInts; run1++ ) {
-acadoWorkspace.rk_xxx[0] = + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + rk_eta[14];
-acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
-acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[2] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[3] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[4] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[5] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[6] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[7] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[8] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[9] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[10] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[11] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 15 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[14];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 30 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[30] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[31] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[32] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[33] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[34] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[35] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[36] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[37] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[38] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[39] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[40] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[41] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[42] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[43] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[44] + rk_eta[14];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 45 ]) );
-rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45];
-rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46];
-rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47];
-rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[48];
-rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[49];
-rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[50];
-rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[51];
-rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[52];
-rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[53];
-rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[54];
-rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[55];
-rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[56];
-rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[57];
-rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[58];
-rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[59];
-acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
-}
-}
-error = 0;
-return error;
-}
-
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp
deleted file mode 100644
index 36e675e03..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-extern "C"
-{
-#include "acado_common.h"
-}
-
-#include "INCLUDE/QProblem.hpp"
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-static int acado_nWSR;
-
-
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-static SolutionAnalysis acado_sa;
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-int acado_solve( void )
-{
-	acado_nWSR = QPOASES_NWSRMAX;
-
-	QProblem qp(23, 20);
-	
-	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
-
-    qp.getPrimalSolution( acadoWorkspace.x );
-    qp.getDualSolution( acadoWorkspace.y );
-	
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-
-	if (retVal != SUCCESSFUL_RETURN)
-		return (int)retVal;
-		
-	retVal = acado_sa.getHessianInverse( &qp,var );
-
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-	return (int)retVal;
-}
-
-int acado_getNWSR( void )
-{
-	return acado_nWSR;
-}
-
-const char* acado_getErrorString( int error )
-{
-	return MessageHandling::getErrorString( error );
-}
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp
deleted file mode 100644
index 1409d1e89..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef QPOASES_HEADER
-#define QPOASES_HEADER
-
-#ifdef PC_DEBUG
-#include <stdio.h>
-#endif /* PC_DEBUG */
-
-#include <math.h>
-
-#ifdef __cplusplus
-#define EXTERNC extern "C"
-#else
-#define EXTERNC
-#endif
-
-/*
- * A set of options for qpOASES
- */
-
-/** Maximum number of optimization variables. */
-#define QPOASES_NVMAX      23
-/** Maximum number of constraints. */
-#define QPOASES_NCMAX      20
-/** Maximum number of working set recalculations. */
-#define QPOASES_NWSRMAX    50
-/** Print level for qpOASES. */
-#define QPOASES_PRINTLEVEL PL_NONE
-/** The value of EPS */
-#define QPOASES_EPS        2.221e-16
-/** Internally used floating point type */
-typedef double real_t;
-
-/*
- * Forward function declarations
- */
-
-/** A function that calls the QP solver */
-EXTERNC int acado_solve( void );
-
-/** Get the number of active set changes */
-EXTERNC int acado_getNWSR( void );
-
-/** Get the error string. */
-const char* acado_getErrorString( int error );
-
-#endif /* QPOASES_HEADER */
diff --git a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c
deleted file mode 100644
index 6dd373b8d..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/lib_mpc_export/acado_solver.c
+++ /dev/null
@@ -1,4782 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-
-
-/******************************************************************************/
-/*                                                                            */
-/* ACADO code generation                                                      */
-/*                                                                            */
-/******************************************************************************/
-
-
-int acado_modelSimulation(  )
-{
-int ret;
-
-int lRun1;
-ret = 0;
-for (lRun1 = 0; lRun1 < 20; ++lRun1)
-{
-acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 3];
-acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 3 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 3 + 2];
-
-acadoWorkspace.state[15] = acadoVariables.u[lRun1];
-acadoWorkspace.state[16] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.state[17] = acadoVariables.od[lRun1 * 2 + 1];
-
-ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
-
-acadoWorkspace.d[lRun1 * 3] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 3 + 3];
-acadoWorkspace.d[lRun1 * 3 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 3 + 4];
-acadoWorkspace.d[lRun1 * 3 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 3 + 5];
-
-acadoWorkspace.evGx[lRun1 * 9] = acadoWorkspace.state[3];
-acadoWorkspace.evGx[lRun1 * 9 + 1] = acadoWorkspace.state[4];
-acadoWorkspace.evGx[lRun1 * 9 + 2] = acadoWorkspace.state[5];
-acadoWorkspace.evGx[lRun1 * 9 + 3] = acadoWorkspace.state[6];
-acadoWorkspace.evGx[lRun1 * 9 + 4] = acadoWorkspace.state[7];
-acadoWorkspace.evGx[lRun1 * 9 + 5] = acadoWorkspace.state[8];
-acadoWorkspace.evGx[lRun1 * 9 + 6] = acadoWorkspace.state[9];
-acadoWorkspace.evGx[lRun1 * 9 + 7] = acadoWorkspace.state[10];
-acadoWorkspace.evGx[lRun1 * 9 + 8] = acadoWorkspace.state[11];
-
-acadoWorkspace.evGu[lRun1 * 3] = acadoWorkspace.state[12];
-acadoWorkspace.evGu[lRun1 * 3 + 1] = acadoWorkspace.state[13];
-acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14];
-}
-return ret;
-}
-
-void acado_evaluateLSQ(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 3;
-const real_t* od = in + 4;
-/* Vector of auxiliary variables; number of elements: 13. */
-real_t* a = acadoWorkspace.objAuxVar;
-
-/* Compute intermediate quantities: */
-a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
-a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
-a[8] = (a[2]*a[2]);
-a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
-a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[12] = (a[10]*a[10]);
-
-/* Compute outputs: */
-out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
-out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
-out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
-out[4] = a[4];
-out[5] = a[9];
-out[6] = (real_t)(0.0000000000000000e+00);
-out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
-out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
-out[9] = (real_t)(0.0000000000000000e+00);
-out[10] = (real_t)(0.0000000000000000e+00);
-out[11] = (xd[2]*(real_t)(1.0000000000000001e-01));
-out[12] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
-out[13] = (real_t)(0.0000000000000000e+00);
-out[14] = (u[0]*(real_t)(1.0000000000000001e-01));
-out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = (real_t)(0.0000000000000000e+00);
-out[18] = (real_t)(0.0000000000000000e+00);
-out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
-}
-
-void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* od = in + 3;
-/* Vector of auxiliary variables; number of elements: 13. */
-real_t* a = acadoWorkspace.objAuxVar;
-
-/* Compute intermediate quantities: */
-a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
-a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
-a[8] = (a[2]*a[2]);
-a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
-a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[12] = (a[10]*a[10]);
-
-/* Compute outputs: */
-out[0] = (a[1]-(real_t)(1.0000000000000000e+00));
-out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
-out[3] = a[4];
-out[4] = a[9];
-out[5] = (real_t)(0.0000000000000000e+00);
-out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]);
-out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12]));
-out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (real_t)(0.0000000000000000e+00);
-out[10] = (xd[2]*(real_t)(1.0000000000000001e-01));
-out[11] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
-}
-
-void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 )
-{
-tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[3]*tmpObjS[4] + tmpFx[6]*tmpObjS[8] + tmpFx[9]*tmpObjS[12];
-tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[3]*tmpObjS[5] + tmpFx[6]*tmpObjS[9] + tmpFx[9]*tmpObjS[13];
-tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[3]*tmpObjS[6] + tmpFx[6]*tmpObjS[10] + tmpFx[9]*tmpObjS[14];
-tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[3]*tmpObjS[7] + tmpFx[6]*tmpObjS[11] + tmpFx[9]*tmpObjS[15];
-tmpQ2[4] = + tmpFx[1]*tmpObjS[0] + tmpFx[4]*tmpObjS[4] + tmpFx[7]*tmpObjS[8] + tmpFx[10]*tmpObjS[12];
-tmpQ2[5] = + tmpFx[1]*tmpObjS[1] + tmpFx[4]*tmpObjS[5] + tmpFx[7]*tmpObjS[9] + tmpFx[10]*tmpObjS[13];
-tmpQ2[6] = + tmpFx[1]*tmpObjS[2] + tmpFx[4]*tmpObjS[6] + tmpFx[7]*tmpObjS[10] + tmpFx[10]*tmpObjS[14];
-tmpQ2[7] = + tmpFx[1]*tmpObjS[3] + tmpFx[4]*tmpObjS[7] + tmpFx[7]*tmpObjS[11] + tmpFx[10]*tmpObjS[15];
-tmpQ2[8] = + tmpFx[2]*tmpObjS[0] + tmpFx[5]*tmpObjS[4] + tmpFx[8]*tmpObjS[8] + tmpFx[11]*tmpObjS[12];
-tmpQ2[9] = + tmpFx[2]*tmpObjS[1] + tmpFx[5]*tmpObjS[5] + tmpFx[8]*tmpObjS[9] + tmpFx[11]*tmpObjS[13];
-tmpQ2[10] = + tmpFx[2]*tmpObjS[2] + tmpFx[5]*tmpObjS[6] + tmpFx[8]*tmpObjS[10] + tmpFx[11]*tmpObjS[14];
-tmpQ2[11] = + tmpFx[2]*tmpObjS[3] + tmpFx[5]*tmpObjS[7] + tmpFx[8]*tmpObjS[11] + tmpFx[11]*tmpObjS[15];
-tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[3] + tmpQ2[2]*tmpFx[6] + tmpQ2[3]*tmpFx[9];
-tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[7] + tmpQ2[3]*tmpFx[10];
-tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[11];
-tmpQ1[3] = + tmpQ2[4]*tmpFx[0] + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[9];
-tmpQ1[4] = + tmpQ2[4]*tmpFx[1] + tmpQ2[5]*tmpFx[4] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[10];
-tmpQ1[5] = + tmpQ2[4]*tmpFx[2] + tmpQ2[5]*tmpFx[5] + tmpQ2[6]*tmpFx[8] + tmpQ2[7]*tmpFx[11];
-tmpQ1[6] = + tmpQ2[8]*tmpFx[0] + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[9];
-tmpQ1[7] = + tmpQ2[8]*tmpFx[1] + tmpQ2[9]*tmpFx[4] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[10];
-tmpQ1[8] = + tmpQ2[8]*tmpFx[2] + tmpQ2[9]*tmpFx[5] + tmpQ2[10]*tmpFx[8] + tmpQ2[11]*tmpFx[11];
-}
-
-void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 )
-{
-tmpR2[0] = + tmpFu[0]*tmpObjS[0] + tmpFu[1]*tmpObjS[4] + tmpFu[2]*tmpObjS[8] + tmpFu[3]*tmpObjS[12];
-tmpR2[1] = + tmpFu[0]*tmpObjS[1] + tmpFu[1]*tmpObjS[5] + tmpFu[2]*tmpObjS[9] + tmpFu[3]*tmpObjS[13];
-tmpR2[2] = + tmpFu[0]*tmpObjS[2] + tmpFu[1]*tmpObjS[6] + tmpFu[2]*tmpObjS[10] + tmpFu[3]*tmpObjS[14];
-tmpR2[3] = + tmpFu[0]*tmpObjS[3] + tmpFu[1]*tmpObjS[7] + tmpFu[2]*tmpObjS[11] + tmpFu[3]*tmpObjS[15];
-tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3]*tmpFu[3];
-}
-
-void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 )
-{
-tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[3]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[6];
-tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[3]*tmpObjSEndTerm[4] + tmpFx[6]*tmpObjSEndTerm[7];
-tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[3]*tmpObjSEndTerm[5] + tmpFx[6]*tmpObjSEndTerm[8];
-tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[6];
-tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[7]*tmpObjSEndTerm[7];
-tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[7]*tmpObjSEndTerm[8];
-tmpQN2[6] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[3] + tmpFx[8]*tmpObjSEndTerm[6];
-tmpQN2[7] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[7];
-tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[8];
-tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[3] + tmpQN2[2]*tmpFx[6];
-tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[7];
-tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[8];
-tmpQN1[3] = + tmpQN2[3]*tmpFx[0] + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[6];
-tmpQN1[4] = + tmpQN2[3]*tmpFx[1] + tmpQN2[4]*tmpFx[4] + tmpQN2[5]*tmpFx[7];
-tmpQN1[5] = + tmpQN2[3]*tmpFx[2] + tmpQN2[4]*tmpFx[5] + tmpQN2[5]*tmpFx[8];
-tmpQN1[6] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[3] + tmpQN2[8]*tmpFx[6];
-tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7];
-tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8];
-}
-
-void acado_evaluateObjective(  )
-{
-int runObj;
-for (runObj = 0; runObj < 20; ++runObj)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 3];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 3 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 3 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1];
-acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2];
-acadoWorkspace.Dy[runObj * 4 + 3] = acadoWorkspace.objValueOut[3];
-
-acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.Q1[ runObj * 9 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) );
-
-acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 16 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) );
-
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[60];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
-acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1];
-acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2];
-
-acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 3 ]), acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 );
-
-}
-
-void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew )
-{
-dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2];
-dNew[1] += + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2];
-dNew[2] += + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2];
-}
-
-void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 )
-{
-Gx2[0] = Gx1[0];
-Gx2[1] = Gx1[1];
-Gx2[2] = Gx1[2];
-Gx2[3] = Gx1[3];
-Gx2[4] = Gx1[4];
-Gx2[5] = Gx1[5];
-Gx2[6] = Gx1[6];
-Gx2[7] = Gx1[7];
-Gx2[8] = Gx1[8];
-}
-
-void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 )
-{
-Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[3] + Gx1[2]*Gx2[6];
-Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[7];
-Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[8];
-Gx3[3] = + Gx1[3]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[6];
-Gx3[4] = + Gx1[3]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[5]*Gx2[7];
-Gx3[5] = + Gx1[3]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[5]*Gx2[8];
-Gx3[6] = + Gx1[6]*Gx2[0] + Gx1[7]*Gx2[3] + Gx1[8]*Gx2[6];
-Gx3[7] = + Gx1[6]*Gx2[1] + Gx1[7]*Gx2[4] + Gx1[8]*Gx2[7];
-Gx3[8] = + Gx1[6]*Gx2[2] + Gx1[7]*Gx2[5] + Gx1[8]*Gx2[8];
-}
-
-void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2];
-Gu2[1] = + Gx1[3]*Gu1[0] + Gx1[4]*Gu1[1] + Gx1[5]*Gu1[2];
-Gu2[2] = + Gx1[6]*Gu1[0] + Gx1[7]*Gu1[1] + Gx1[8]*Gu1[2];
-}
-
-void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = Gu1[0];
-Gu2[1] = Gu1[1];
-Gu2[2] = Gu1[2];
-}
-
-void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 )
-{
-acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2];
-}
-
-void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 )
-{
-acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = R11[0];
-}
-
-void acado_zeroBlockH11( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = 0.0000000000000000e+00;
-}
-
-void acado_copyHTH( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = acadoWorkspace.H[(iCol * 23 + 69) + (iRow + 3)];
-}
-
-void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2];
-dNew[1] = + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2];
-dNew[2] = + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2];
-}
-
-void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2];
-dNew[1] = + acadoWorkspace.QN1[3]*dOld[0] + acadoWorkspace.QN1[4]*dOld[1] + acadoWorkspace.QN1[5]*dOld[2];
-dNew[2] = + acadoWorkspace.QN1[6]*dOld[0] + acadoWorkspace.QN1[7]*dOld[1] + acadoWorkspace.QN1[8]*dOld[2];
-}
-
-void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 )
-{
-RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3];
-}
-
-void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 )
-{
-QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3];
-QDy1[1] = + Q2[4]*Dy1[0] + Q2[5]*Dy1[1] + Q2[6]*Dy1[2] + Q2[7]*Dy1[3];
-QDy1[2] = + Q2[8]*Dy1[0] + Q2[9]*Dy1[1] + Q2[10]*Dy1[2] + Q2[11]*Dy1[3];
-}
-
-void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 )
-{
-U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2];
-}
-
-void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 )
-{
-H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[3] + E1[2]*Gx1[6];
-H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[4] + E1[2]*Gx1[7];
-H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[5] + E1[2]*Gx1[8];
-}
-
-void acado_zeroBlockH10( real_t* const H101 )
-{
-{ int lCopy; for (lCopy = 0; lCopy < 3; lCopy++) H101[ lCopy ] = 0; }
-}
-
-void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew )
-{
-dNew[0] += + E1[0]*U1[0];
-dNew[1] += + E1[1]*U1[0];
-dNew[2] += + E1[2]*U1[0];
-}
-
-void acado_zeroBlockH00(  )
-{
-acadoWorkspace.H[0] = 0.0000000000000000e+00;
-acadoWorkspace.H[1] = 0.0000000000000000e+00;
-acadoWorkspace.H[2] = 0.0000000000000000e+00;
-acadoWorkspace.H[23] = 0.0000000000000000e+00;
-acadoWorkspace.H[24] = 0.0000000000000000e+00;
-acadoWorkspace.H[25] = 0.0000000000000000e+00;
-acadoWorkspace.H[46] = 0.0000000000000000e+00;
-acadoWorkspace.H[47] = 0.0000000000000000e+00;
-acadoWorkspace.H[48] = 0.0000000000000000e+00;
-}
-
-void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 )
-{
-acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[3]*Gx2[3] + Gx1[6]*Gx2[6];
-acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[3]*Gx2[4] + Gx1[6]*Gx2[7];
-acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[3]*Gx2[5] + Gx1[6]*Gx2[8];
-acadoWorkspace.H[23] += + Gx1[1]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[7]*Gx2[6];
-acadoWorkspace.H[24] += + Gx1[1]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[7]*Gx2[7];
-acadoWorkspace.H[25] += + Gx1[1]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[7]*Gx2[8];
-acadoWorkspace.H[46] += + Gx1[2]*Gx2[0] + Gx1[5]*Gx2[3] + Gx1[8]*Gx2[6];
-acadoWorkspace.H[47] += + Gx1[2]*Gx2[1] + Gx1[5]*Gx2[4] + Gx1[8]*Gx2[7];
-acadoWorkspace.H[48] += + Gx1[2]*Gx2[2] + Gx1[5]*Gx2[5] + Gx1[8]*Gx2[8];
-}
-
-void acado_macCTSlx( real_t* const C0, real_t* const g0 )
-{
-g0[0] += 0.0;
-;
-g0[1] += 0.0;
-;
-g0[2] += 0.0;
-;
-}
-
-void acado_macETSlu( real_t* const E0, real_t* const g1 )
-{
-g1[0] += 0.0;
-;
-}
-
-void acado_condensePrep(  )
-{
-int lRun1;
-int lRun2;
-int lRun3;
-int lRun4;
-int lRun5;
-/** Row vector of size: 20 */
-static const int xBoundIndices[ 20 ] = 
-{ 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 };
-acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E );
-acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T );
-acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.d[ 3 ]) );
-acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 9 ]) );
-
-acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 3 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 3 ]), &(acadoWorkspace.E[ 6 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.d[ 6 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.evGx[ 18 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.E[ 9 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.E[ 12 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 6 ]), &(acadoWorkspace.E[ 15 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.d[ 9 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.evGx[ 27 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.E[ 18 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 21 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.E[ 24 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 9 ]), &(acadoWorkspace.E[ 27 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.d[ 12 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.evGx[ 36 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.E[ 30 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.E[ 33 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 36 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.E[ 39 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 42 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.d[ 15 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.evGx[ 45 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.E[ 45 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.E[ 48 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 51 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.E[ 54 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.E[ 57 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 15 ]), &(acadoWorkspace.E[ 60 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.d[ 18 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.evGx[ 54 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.E[ 63 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 66 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.E[ 69 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.E[ 72 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.E[ 75 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 78 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 18 ]), &(acadoWorkspace.E[ 81 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.d[ 21 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.evGx[ 63 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.E[ 84 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.E[ 87 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.E[ 90 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 93 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.E[ 96 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.E[ 99 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.E[ 102 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 21 ]), &(acadoWorkspace.E[ 105 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.d[ 24 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.evGx[ 72 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 108 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.E[ 111 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.E[ 114 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.E[ 117 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 120 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.E[ 123 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.E[ 126 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.E[ 129 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 132 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.d[ 27 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.evGx[ 81 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 135 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.E[ 138 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.E[ 141 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.E[ 144 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 147 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.E[ 150 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.E[ 153 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.E[ 156 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 159 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 27 ]), &(acadoWorkspace.E[ 162 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.d[ 30 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.evGx[ 90 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.E[ 165 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.E[ 168 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.E[ 171 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 174 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.E[ 177 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.E[ 180 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.E[ 183 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 186 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.E[ 189 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.E[ 192 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 30 ]), &(acadoWorkspace.E[ 195 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.d[ 33 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.evGx[ 99 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.E[ 198 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 201 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.E[ 204 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.E[ 207 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.E[ 210 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 213 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.E[ 216 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.E[ 219 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.E[ 222 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 225 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.E[ 228 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 33 ]), &(acadoWorkspace.E[ 231 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.d[ 36 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.evGx[ 108 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.E[ 234 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.E[ 237 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 240 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.E[ 243 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.E[ 246 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.E[ 249 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 252 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.E[ 255 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.E[ 258 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.E[ 261 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 264 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.E[ 267 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 270 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.d[ 39 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.evGx[ 117 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.E[ 273 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.E[ 276 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 279 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.E[ 282 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.E[ 285 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.E[ 288 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 291 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.E[ 294 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.E[ 297 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.E[ 300 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 303 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.E[ 306 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.E[ 309 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 39 ]), &(acadoWorkspace.E[ 312 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.d[ 42 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.evGx[ 126 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.E[ 315 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 318 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.E[ 321 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.E[ 324 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.E[ 327 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 330 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.E[ 333 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.E[ 336 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.E[ 339 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 342 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.E[ 345 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.E[ 348 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.E[ 351 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 354 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 42 ]), &(acadoWorkspace.E[ 357 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.d[ 45 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.evGx[ 135 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.E[ 360 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.E[ 363 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.E[ 366 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 369 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.E[ 372 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.E[ 375 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.E[ 378 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 381 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.E[ 384 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.E[ 387 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.E[ 390 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 393 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.E[ 396 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.E[ 399 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.E[ 402 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 45 ]), &(acadoWorkspace.E[ 405 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 48 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.evGx[ 144 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 408 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.E[ 411 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.E[ 414 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.E[ 417 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 420 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.E[ 423 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.E[ 426 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.E[ 429 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 432 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.E[ 435 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.E[ 438 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.E[ 441 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 444 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.E[ 447 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.E[ 450 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.E[ 453 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 456 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.d[ 51 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 153 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 459 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.E[ 462 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.E[ 465 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.E[ 468 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 471 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.E[ 474 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.E[ 477 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.E[ 480 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 483 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.E[ 486 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.E[ 489 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.E[ 492 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 495 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.E[ 498 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.E[ 501 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.E[ 504 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 507 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 51 ]), &(acadoWorkspace.E[ 510 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.d[ 54 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.evGx[ 162 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.E[ 513 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.E[ 516 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.E[ 519 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 522 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.E[ 525 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.E[ 528 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.E[ 531 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 534 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.E[ 537 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.E[ 540 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.E[ 543 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 546 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.E[ 549 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.E[ 552 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.E[ 555 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 558 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.E[ 561 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.E[ 564 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 54 ]), &(acadoWorkspace.E[ 567 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.d[ 57 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.evGx[ 171 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.E[ 570 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 573 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.E[ 576 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.E[ 579 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.E[ 582 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 585 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.E[ 588 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.E[ 591 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.E[ 594 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 597 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.E[ 600 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.E[ 603 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.E[ 606 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 609 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.E[ 612 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.E[ 615 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.E[ 618 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 621 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.E[ 624 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 57 ]), &(acadoWorkspace.E[ 627 ]) );
-
-acado_multGxGx( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multGxGx( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) );
-acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) );
-
-acado_multGxGu( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.E, acadoWorkspace.QE );
-acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_zeroBlockH00(  );
-acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multCTQC( &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) );
-
-acado_zeroBlockH10( acadoWorkspace.H10 );
-acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.H10 );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 3 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 9 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 15 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 21 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 27 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 33 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 39 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 45 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 45 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 45 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 45 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 45 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 45 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 51 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 51 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 51 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 51 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 57 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 57 ]) );
-
-acadoWorkspace.H[3] = acadoWorkspace.H10[0];
-acadoWorkspace.H[4] = acadoWorkspace.H10[3];
-acadoWorkspace.H[5] = acadoWorkspace.H10[6];
-acadoWorkspace.H[6] = acadoWorkspace.H10[9];
-acadoWorkspace.H[7] = acadoWorkspace.H10[12];
-acadoWorkspace.H[8] = acadoWorkspace.H10[15];
-acadoWorkspace.H[9] = acadoWorkspace.H10[18];
-acadoWorkspace.H[10] = acadoWorkspace.H10[21];
-acadoWorkspace.H[11] = acadoWorkspace.H10[24];
-acadoWorkspace.H[12] = acadoWorkspace.H10[27];
-acadoWorkspace.H[13] = acadoWorkspace.H10[30];
-acadoWorkspace.H[14] = acadoWorkspace.H10[33];
-acadoWorkspace.H[15] = acadoWorkspace.H10[36];
-acadoWorkspace.H[16] = acadoWorkspace.H10[39];
-acadoWorkspace.H[17] = acadoWorkspace.H10[42];
-acadoWorkspace.H[18] = acadoWorkspace.H10[45];
-acadoWorkspace.H[19] = acadoWorkspace.H10[48];
-acadoWorkspace.H[20] = acadoWorkspace.H10[51];
-acadoWorkspace.H[21] = acadoWorkspace.H10[54];
-acadoWorkspace.H[22] = acadoWorkspace.H10[57];
-acadoWorkspace.H[26] = acadoWorkspace.H10[1];
-acadoWorkspace.H[27] = acadoWorkspace.H10[4];
-acadoWorkspace.H[28] = acadoWorkspace.H10[7];
-acadoWorkspace.H[29] = acadoWorkspace.H10[10];
-acadoWorkspace.H[30] = acadoWorkspace.H10[13];
-acadoWorkspace.H[31] = acadoWorkspace.H10[16];
-acadoWorkspace.H[32] = acadoWorkspace.H10[19];
-acadoWorkspace.H[33] = acadoWorkspace.H10[22];
-acadoWorkspace.H[34] = acadoWorkspace.H10[25];
-acadoWorkspace.H[35] = acadoWorkspace.H10[28];
-acadoWorkspace.H[36] = acadoWorkspace.H10[31];
-acadoWorkspace.H[37] = acadoWorkspace.H10[34];
-acadoWorkspace.H[38] = acadoWorkspace.H10[37];
-acadoWorkspace.H[39] = acadoWorkspace.H10[40];
-acadoWorkspace.H[40] = acadoWorkspace.H10[43];
-acadoWorkspace.H[41] = acadoWorkspace.H10[46];
-acadoWorkspace.H[42] = acadoWorkspace.H10[49];
-acadoWorkspace.H[43] = acadoWorkspace.H10[52];
-acadoWorkspace.H[44] = acadoWorkspace.H10[55];
-acadoWorkspace.H[45] = acadoWorkspace.H10[58];
-acadoWorkspace.H[49] = acadoWorkspace.H10[2];
-acadoWorkspace.H[50] = acadoWorkspace.H10[5];
-acadoWorkspace.H[51] = acadoWorkspace.H10[8];
-acadoWorkspace.H[52] = acadoWorkspace.H10[11];
-acadoWorkspace.H[53] = acadoWorkspace.H10[14];
-acadoWorkspace.H[54] = acadoWorkspace.H10[17];
-acadoWorkspace.H[55] = acadoWorkspace.H10[20];
-acadoWorkspace.H[56] = acadoWorkspace.H10[23];
-acadoWorkspace.H[57] = acadoWorkspace.H10[26];
-acadoWorkspace.H[58] = acadoWorkspace.H10[29];
-acadoWorkspace.H[59] = acadoWorkspace.H10[32];
-acadoWorkspace.H[60] = acadoWorkspace.H10[35];
-acadoWorkspace.H[61] = acadoWorkspace.H10[38];
-acadoWorkspace.H[62] = acadoWorkspace.H10[41];
-acadoWorkspace.H[63] = acadoWorkspace.H10[44];
-acadoWorkspace.H[64] = acadoWorkspace.H10[47];
-acadoWorkspace.H[65] = acadoWorkspace.H10[50];
-acadoWorkspace.H[66] = acadoWorkspace.H10[53];
-acadoWorkspace.H[67] = acadoWorkspace.H10[56];
-acadoWorkspace.H[68] = acadoWorkspace.H10[59];
-
-acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 );
-acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
-
-acado_zeroBlockH11( 0, 1 );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 6 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 21 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 33 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 87 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 111 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 201 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 237 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 363 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 411 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 573 ]) );
-
-acado_zeroBlockH11( 0, 2 );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 15 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 51 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 69 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 141 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 171 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 279 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 321 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 465 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 519 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 576 ]) );
-
-acado_zeroBlockH11( 0, 3 );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 27 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 39 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 93 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 117 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 207 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 243 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 369 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 417 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 579 ]) );
-
-acado_zeroBlockH11( 0, 4 );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 582 ]) );
-
-acado_zeroBlockH11( 0, 5 );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 0, 6 );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 0, 7 );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 0, 8 );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 0, 9 );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 0, 10 );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 0, 11 );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 0, 12 );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 0, 13 );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 0, 14 );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 0, 15 );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 0, 16 );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 0, 17 );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 0, 18 );
-acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 0, 19 );
-acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) );
-
-acado_zeroBlockH11( 1, 2 );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 15 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 51 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 69 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 141 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 171 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 279 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 321 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 465 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 519 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 576 ]) );
-
-acado_zeroBlockH11( 1, 3 );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 27 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 39 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 93 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 117 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 207 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 243 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 369 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 417 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 579 ]) );
-
-acado_zeroBlockH11( 1, 4 );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 582 ]) );
-
-acado_zeroBlockH11( 1, 5 );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 1, 6 );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 1, 7 );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 1, 8 );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 1, 9 );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 1, 10 );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 1, 11 );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 1, 12 );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 1, 13 );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 1, 14 );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 1, 15 );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 1, 16 );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 1, 17 );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 1, 18 );
-acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 1, 19 );
-acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
-
-acado_zeroBlockH11( 2, 3 );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 27 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 39 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 93 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 117 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 207 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 243 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 369 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 417 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 579 ]) );
-
-acado_zeroBlockH11( 2, 4 );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 582 ]) );
-
-acado_zeroBlockH11( 2, 5 );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 2, 6 );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 2, 7 );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 2, 8 );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 2, 9 );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 2, 10 );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 2, 11 );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 2, 12 );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 2, 13 );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 2, 14 );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 2, 15 );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 2, 16 );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 2, 17 );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 2, 18 );
-acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 2, 19 );
-acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) );
-
-acado_zeroBlockH11( 3, 4 );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 582 ]) );
-
-acado_zeroBlockH11( 3, 5 );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 3, 6 );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 3, 7 );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 3, 8 );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 3, 9 );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 3, 10 );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 3, 11 );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 3, 12 );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 3, 13 );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 3, 14 );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 3, 15 );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 3, 16 );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 3, 17 );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 3, 18 );
-acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 3, 19 );
-acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
-
-acado_zeroBlockH11( 4, 5 );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 4, 6 );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 4, 7 );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 4, 8 );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 4, 9 );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 4, 10 );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 4, 11 );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 4, 12 );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 4, 13 );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 4, 14 );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 4, 15 );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 4, 16 );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 4, 17 );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 4, 18 );
-acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 4, 19 );
-acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) );
-
-acado_zeroBlockH11( 5, 6 );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 5, 7 );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 5, 8 );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 5, 9 );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 5, 10 );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 5, 11 );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 5, 12 );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 5, 13 );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 5, 14 );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 5, 15 );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 5, 16 );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 5, 17 );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 5, 18 );
-acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 5, 19 );
-acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
-
-acado_zeroBlockH11( 6, 7 );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 6, 8 );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 6, 9 );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 6, 10 );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 6, 11 );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 6, 12 );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 6, 13 );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 6, 14 );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 6, 15 );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 6, 16 );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 6, 17 );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 6, 18 );
-acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 6, 19 );
-acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) );
-
-acado_zeroBlockH11( 7, 8 );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 7, 9 );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 7, 10 );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 7, 11 );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 7, 12 );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 7, 13 );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 7, 14 );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 7, 15 );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 7, 16 );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 7, 17 );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 7, 18 );
-acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 7, 19 );
-acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) );
-
-acado_zeroBlockH11( 8, 9 );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 8, 10 );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 8, 11 );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 8, 12 );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 8, 13 );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 8, 14 );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 8, 15 );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 8, 16 );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 8, 17 );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 8, 18 );
-acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 8, 19 );
-acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) );
-
-acado_zeroBlockH11( 9, 10 );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 9, 11 );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 9, 12 );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 9, 13 );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 9, 14 );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 9, 15 );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 9, 16 );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 9, 17 );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 9, 18 );
-acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 9, 19 );
-acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
-
-acado_zeroBlockH11( 10, 11 );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 10, 12 );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 10, 13 );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 10, 14 );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 10, 15 );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 10, 16 );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 10, 17 );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 10, 18 );
-acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 10, 19 );
-acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) );
-
-acado_zeroBlockH11( 11, 12 );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 11, 13 );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 11, 14 );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 11, 15 );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 11, 16 );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 11, 17 );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 11, 18 );
-acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 11, 19 );
-acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
-
-acado_zeroBlockH11( 12, 13 );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 12, 14 );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 12, 15 );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 12, 16 );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 12, 17 );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 12, 18 );
-acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 12, 19 );
-acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) );
-
-acado_zeroBlockH11( 13, 14 );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 13, 15 );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 13, 16 );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 13, 17 );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 13, 18 );
-acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 13, 19 );
-acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
-
-acado_zeroBlockH11( 14, 15 );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 14, 16 );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 14, 17 );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 14, 18 );
-acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 14, 19 );
-acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) );
-
-acado_zeroBlockH11( 15, 16 );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 15, 17 );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 15, 18 );
-acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 15, 19 );
-acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
-
-acado_zeroBlockH11( 16, 17 );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 16, 18 );
-acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 16, 19 );
-acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) );
-
-acado_zeroBlockH11( 17, 18 );
-acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 17, 19 );
-acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) );
-acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) );
-acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
-
-acado_zeroBlockH11( 18, 19 );
-acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) );
-acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) );
-
-
-acado_copyHTH( 1, 0 );
-acado_copyHTH( 2, 0 );
-acado_copyHTH( 2, 1 );
-acado_copyHTH( 3, 0 );
-acado_copyHTH( 3, 1 );
-acado_copyHTH( 3, 2 );
-acado_copyHTH( 4, 0 );
-acado_copyHTH( 4, 1 );
-acado_copyHTH( 4, 2 );
-acado_copyHTH( 4, 3 );
-acado_copyHTH( 5, 0 );
-acado_copyHTH( 5, 1 );
-acado_copyHTH( 5, 2 );
-acado_copyHTH( 5, 3 );
-acado_copyHTH( 5, 4 );
-acado_copyHTH( 6, 0 );
-acado_copyHTH( 6, 1 );
-acado_copyHTH( 6, 2 );
-acado_copyHTH( 6, 3 );
-acado_copyHTH( 6, 4 );
-acado_copyHTH( 6, 5 );
-acado_copyHTH( 7, 0 );
-acado_copyHTH( 7, 1 );
-acado_copyHTH( 7, 2 );
-acado_copyHTH( 7, 3 );
-acado_copyHTH( 7, 4 );
-acado_copyHTH( 7, 5 );
-acado_copyHTH( 7, 6 );
-acado_copyHTH( 8, 0 );
-acado_copyHTH( 8, 1 );
-acado_copyHTH( 8, 2 );
-acado_copyHTH( 8, 3 );
-acado_copyHTH( 8, 4 );
-acado_copyHTH( 8, 5 );
-acado_copyHTH( 8, 6 );
-acado_copyHTH( 8, 7 );
-acado_copyHTH( 9, 0 );
-acado_copyHTH( 9, 1 );
-acado_copyHTH( 9, 2 );
-acado_copyHTH( 9, 3 );
-acado_copyHTH( 9, 4 );
-acado_copyHTH( 9, 5 );
-acado_copyHTH( 9, 6 );
-acado_copyHTH( 9, 7 );
-acado_copyHTH( 9, 8 );
-acado_copyHTH( 10, 0 );
-acado_copyHTH( 10, 1 );
-acado_copyHTH( 10, 2 );
-acado_copyHTH( 10, 3 );
-acado_copyHTH( 10, 4 );
-acado_copyHTH( 10, 5 );
-acado_copyHTH( 10, 6 );
-acado_copyHTH( 10, 7 );
-acado_copyHTH( 10, 8 );
-acado_copyHTH( 10, 9 );
-acado_copyHTH( 11, 0 );
-acado_copyHTH( 11, 1 );
-acado_copyHTH( 11, 2 );
-acado_copyHTH( 11, 3 );
-acado_copyHTH( 11, 4 );
-acado_copyHTH( 11, 5 );
-acado_copyHTH( 11, 6 );
-acado_copyHTH( 11, 7 );
-acado_copyHTH( 11, 8 );
-acado_copyHTH( 11, 9 );
-acado_copyHTH( 11, 10 );
-acado_copyHTH( 12, 0 );
-acado_copyHTH( 12, 1 );
-acado_copyHTH( 12, 2 );
-acado_copyHTH( 12, 3 );
-acado_copyHTH( 12, 4 );
-acado_copyHTH( 12, 5 );
-acado_copyHTH( 12, 6 );
-acado_copyHTH( 12, 7 );
-acado_copyHTH( 12, 8 );
-acado_copyHTH( 12, 9 );
-acado_copyHTH( 12, 10 );
-acado_copyHTH( 12, 11 );
-acado_copyHTH( 13, 0 );
-acado_copyHTH( 13, 1 );
-acado_copyHTH( 13, 2 );
-acado_copyHTH( 13, 3 );
-acado_copyHTH( 13, 4 );
-acado_copyHTH( 13, 5 );
-acado_copyHTH( 13, 6 );
-acado_copyHTH( 13, 7 );
-acado_copyHTH( 13, 8 );
-acado_copyHTH( 13, 9 );
-acado_copyHTH( 13, 10 );
-acado_copyHTH( 13, 11 );
-acado_copyHTH( 13, 12 );
-acado_copyHTH( 14, 0 );
-acado_copyHTH( 14, 1 );
-acado_copyHTH( 14, 2 );
-acado_copyHTH( 14, 3 );
-acado_copyHTH( 14, 4 );
-acado_copyHTH( 14, 5 );
-acado_copyHTH( 14, 6 );
-acado_copyHTH( 14, 7 );
-acado_copyHTH( 14, 8 );
-acado_copyHTH( 14, 9 );
-acado_copyHTH( 14, 10 );
-acado_copyHTH( 14, 11 );
-acado_copyHTH( 14, 12 );
-acado_copyHTH( 14, 13 );
-acado_copyHTH( 15, 0 );
-acado_copyHTH( 15, 1 );
-acado_copyHTH( 15, 2 );
-acado_copyHTH( 15, 3 );
-acado_copyHTH( 15, 4 );
-acado_copyHTH( 15, 5 );
-acado_copyHTH( 15, 6 );
-acado_copyHTH( 15, 7 );
-acado_copyHTH( 15, 8 );
-acado_copyHTH( 15, 9 );
-acado_copyHTH( 15, 10 );
-acado_copyHTH( 15, 11 );
-acado_copyHTH( 15, 12 );
-acado_copyHTH( 15, 13 );
-acado_copyHTH( 15, 14 );
-acado_copyHTH( 16, 0 );
-acado_copyHTH( 16, 1 );
-acado_copyHTH( 16, 2 );
-acado_copyHTH( 16, 3 );
-acado_copyHTH( 16, 4 );
-acado_copyHTH( 16, 5 );
-acado_copyHTH( 16, 6 );
-acado_copyHTH( 16, 7 );
-acado_copyHTH( 16, 8 );
-acado_copyHTH( 16, 9 );
-acado_copyHTH( 16, 10 );
-acado_copyHTH( 16, 11 );
-acado_copyHTH( 16, 12 );
-acado_copyHTH( 16, 13 );
-acado_copyHTH( 16, 14 );
-acado_copyHTH( 16, 15 );
-acado_copyHTH( 17, 0 );
-acado_copyHTH( 17, 1 );
-acado_copyHTH( 17, 2 );
-acado_copyHTH( 17, 3 );
-acado_copyHTH( 17, 4 );
-acado_copyHTH( 17, 5 );
-acado_copyHTH( 17, 6 );
-acado_copyHTH( 17, 7 );
-acado_copyHTH( 17, 8 );
-acado_copyHTH( 17, 9 );
-acado_copyHTH( 17, 10 );
-acado_copyHTH( 17, 11 );
-acado_copyHTH( 17, 12 );
-acado_copyHTH( 17, 13 );
-acado_copyHTH( 17, 14 );
-acado_copyHTH( 17, 15 );
-acado_copyHTH( 17, 16 );
-acado_copyHTH( 18, 0 );
-acado_copyHTH( 18, 1 );
-acado_copyHTH( 18, 2 );
-acado_copyHTH( 18, 3 );
-acado_copyHTH( 18, 4 );
-acado_copyHTH( 18, 5 );
-acado_copyHTH( 18, 6 );
-acado_copyHTH( 18, 7 );
-acado_copyHTH( 18, 8 );
-acado_copyHTH( 18, 9 );
-acado_copyHTH( 18, 10 );
-acado_copyHTH( 18, 11 );
-acado_copyHTH( 18, 12 );
-acado_copyHTH( 18, 13 );
-acado_copyHTH( 18, 14 );
-acado_copyHTH( 18, 15 );
-acado_copyHTH( 18, 16 );
-acado_copyHTH( 18, 17 );
-acado_copyHTH( 19, 0 );
-acado_copyHTH( 19, 1 );
-acado_copyHTH( 19, 2 );
-acado_copyHTH( 19, 3 );
-acado_copyHTH( 19, 4 );
-acado_copyHTH( 19, 5 );
-acado_copyHTH( 19, 6 );
-acado_copyHTH( 19, 7 );
-acado_copyHTH( 19, 8 );
-acado_copyHTH( 19, 9 );
-acado_copyHTH( 19, 10 );
-acado_copyHTH( 19, 11 );
-acado_copyHTH( 19, 12 );
-acado_copyHTH( 19, 13 );
-acado_copyHTH( 19, 14 );
-acado_copyHTH( 19, 15 );
-acado_copyHTH( 19, 16 );
-acado_copyHTH( 19, 17 );
-acado_copyHTH( 19, 18 );
-
-acadoWorkspace.H[69] = acadoWorkspace.H10[0];
-acadoWorkspace.H[70] = acadoWorkspace.H10[1];
-acadoWorkspace.H[71] = acadoWorkspace.H10[2];
-acadoWorkspace.H[92] = acadoWorkspace.H10[3];
-acadoWorkspace.H[93] = acadoWorkspace.H10[4];
-acadoWorkspace.H[94] = acadoWorkspace.H10[5];
-acadoWorkspace.H[115] = acadoWorkspace.H10[6];
-acadoWorkspace.H[116] = acadoWorkspace.H10[7];
-acadoWorkspace.H[117] = acadoWorkspace.H10[8];
-acadoWorkspace.H[138] = acadoWorkspace.H10[9];
-acadoWorkspace.H[139] = acadoWorkspace.H10[10];
-acadoWorkspace.H[140] = acadoWorkspace.H10[11];
-acadoWorkspace.H[161] = acadoWorkspace.H10[12];
-acadoWorkspace.H[162] = acadoWorkspace.H10[13];
-acadoWorkspace.H[163] = acadoWorkspace.H10[14];
-acadoWorkspace.H[184] = acadoWorkspace.H10[15];
-acadoWorkspace.H[185] = acadoWorkspace.H10[16];
-acadoWorkspace.H[186] = acadoWorkspace.H10[17];
-acadoWorkspace.H[207] = acadoWorkspace.H10[18];
-acadoWorkspace.H[208] = acadoWorkspace.H10[19];
-acadoWorkspace.H[209] = acadoWorkspace.H10[20];
-acadoWorkspace.H[230] = acadoWorkspace.H10[21];
-acadoWorkspace.H[231] = acadoWorkspace.H10[22];
-acadoWorkspace.H[232] = acadoWorkspace.H10[23];
-acadoWorkspace.H[253] = acadoWorkspace.H10[24];
-acadoWorkspace.H[254] = acadoWorkspace.H10[25];
-acadoWorkspace.H[255] = acadoWorkspace.H10[26];
-acadoWorkspace.H[276] = acadoWorkspace.H10[27];
-acadoWorkspace.H[277] = acadoWorkspace.H10[28];
-acadoWorkspace.H[278] = acadoWorkspace.H10[29];
-acadoWorkspace.H[299] = acadoWorkspace.H10[30];
-acadoWorkspace.H[300] = acadoWorkspace.H10[31];
-acadoWorkspace.H[301] = acadoWorkspace.H10[32];
-acadoWorkspace.H[322] = acadoWorkspace.H10[33];
-acadoWorkspace.H[323] = acadoWorkspace.H10[34];
-acadoWorkspace.H[324] = acadoWorkspace.H10[35];
-acadoWorkspace.H[345] = acadoWorkspace.H10[36];
-acadoWorkspace.H[346] = acadoWorkspace.H10[37];
-acadoWorkspace.H[347] = acadoWorkspace.H10[38];
-acadoWorkspace.H[368] = acadoWorkspace.H10[39];
-acadoWorkspace.H[369] = acadoWorkspace.H10[40];
-acadoWorkspace.H[370] = acadoWorkspace.H10[41];
-acadoWorkspace.H[391] = acadoWorkspace.H10[42];
-acadoWorkspace.H[392] = acadoWorkspace.H10[43];
-acadoWorkspace.H[393] = acadoWorkspace.H10[44];
-acadoWorkspace.H[414] = acadoWorkspace.H10[45];
-acadoWorkspace.H[415] = acadoWorkspace.H10[46];
-acadoWorkspace.H[416] = acadoWorkspace.H10[47];
-acadoWorkspace.H[437] = acadoWorkspace.H10[48];
-acadoWorkspace.H[438] = acadoWorkspace.H10[49];
-acadoWorkspace.H[439] = acadoWorkspace.H10[50];
-acadoWorkspace.H[460] = acadoWorkspace.H10[51];
-acadoWorkspace.H[461] = acadoWorkspace.H10[52];
-acadoWorkspace.H[462] = acadoWorkspace.H10[53];
-acadoWorkspace.H[483] = acadoWorkspace.H10[54];
-acadoWorkspace.H[484] = acadoWorkspace.H10[55];
-acadoWorkspace.H[485] = acadoWorkspace.H10[56];
-acadoWorkspace.H[506] = acadoWorkspace.H10[57];
-acadoWorkspace.H[507] = acadoWorkspace.H10[58];
-acadoWorkspace.H[508] = acadoWorkspace.H10[59];
-
-acado_multQ1d( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.d, acadoWorkspace.Qd );
-acado_multQ1d( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.Qd[ 3 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.Qd[ 6 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.Qd[ 9 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.Qd[ 15 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.Qd[ 18 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.Qd[ 21 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.Qd[ 27 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.Qd[ 30 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.Qd[ 33 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.Qd[ 39 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.Qd[ 42 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.Qd[ 45 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.Qd[ 51 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.Qd[ 54 ]) );
-acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 57 ]), &(acadoWorkspace.Qd[ 57 ]) );
-
-acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.g );
-acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.g[ 22 ]) );
-acadoWorkspace.lb[3] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
-acadoWorkspace.ub[3] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
-
-for (lRun1 = 0; lRun1 < 20; ++lRun1)
-{
-lRun3 = xBoundIndices[ lRun1 ] - 3;
-lRun4 = ((lRun3) / (3)) + (1);
-acadoWorkspace.A[lRun1 * 23] = acadoWorkspace.evGx[lRun3 * 3];
-acadoWorkspace.A[lRun1 * 23 + 1] = acadoWorkspace.evGx[lRun3 * 3 + 1];
-acadoWorkspace.A[lRun1 * 23 + 2] = acadoWorkspace.evGx[lRun3 * 3 + 2];
-for (lRun2 = 0; lRun2 < lRun4; ++lRun2)
-{
-lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (3)) + ((lRun3) % (3));
-acadoWorkspace.A[(lRun1 * 23) + (lRun2 + 3)] = acadoWorkspace.E[lRun5];
-}
-}
-
-}
-
-void acado_condenseFdb(  )
-{
-real_t tmp;
-
-acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0];
-acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1];
-acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2];
-
-acadoWorkspace.Dy[0] -= acadoVariables.y[0];
-acadoWorkspace.Dy[1] -= acadoVariables.y[1];
-acadoWorkspace.Dy[2] -= acadoVariables.y[2];
-acadoWorkspace.Dy[3] -= acadoVariables.y[3];
-acadoWorkspace.Dy[4] -= acadoVariables.y[4];
-acadoWorkspace.Dy[5] -= acadoVariables.y[5];
-acadoWorkspace.Dy[6] -= acadoVariables.y[6];
-acadoWorkspace.Dy[7] -= acadoVariables.y[7];
-acadoWorkspace.Dy[8] -= acadoVariables.y[8];
-acadoWorkspace.Dy[9] -= acadoVariables.y[9];
-acadoWorkspace.Dy[10] -= acadoVariables.y[10];
-acadoWorkspace.Dy[11] -= acadoVariables.y[11];
-acadoWorkspace.Dy[12] -= acadoVariables.y[12];
-acadoWorkspace.Dy[13] -= acadoVariables.y[13];
-acadoWorkspace.Dy[14] -= acadoVariables.y[14];
-acadoWorkspace.Dy[15] -= acadoVariables.y[15];
-acadoWorkspace.Dy[16] -= acadoVariables.y[16];
-acadoWorkspace.Dy[17] -= acadoVariables.y[17];
-acadoWorkspace.Dy[18] -= acadoVariables.y[18];
-acadoWorkspace.Dy[19] -= acadoVariables.y[19];
-acadoWorkspace.Dy[20] -= acadoVariables.y[20];
-acadoWorkspace.Dy[21] -= acadoVariables.y[21];
-acadoWorkspace.Dy[22] -= acadoVariables.y[22];
-acadoWorkspace.Dy[23] -= acadoVariables.y[23];
-acadoWorkspace.Dy[24] -= acadoVariables.y[24];
-acadoWorkspace.Dy[25] -= acadoVariables.y[25];
-acadoWorkspace.Dy[26] -= acadoVariables.y[26];
-acadoWorkspace.Dy[27] -= acadoVariables.y[27];
-acadoWorkspace.Dy[28] -= acadoVariables.y[28];
-acadoWorkspace.Dy[29] -= acadoVariables.y[29];
-acadoWorkspace.Dy[30] -= acadoVariables.y[30];
-acadoWorkspace.Dy[31] -= acadoVariables.y[31];
-acadoWorkspace.Dy[32] -= acadoVariables.y[32];
-acadoWorkspace.Dy[33] -= acadoVariables.y[33];
-acadoWorkspace.Dy[34] -= acadoVariables.y[34];
-acadoWorkspace.Dy[35] -= acadoVariables.y[35];
-acadoWorkspace.Dy[36] -= acadoVariables.y[36];
-acadoWorkspace.Dy[37] -= acadoVariables.y[37];
-acadoWorkspace.Dy[38] -= acadoVariables.y[38];
-acadoWorkspace.Dy[39] -= acadoVariables.y[39];
-acadoWorkspace.Dy[40] -= acadoVariables.y[40];
-acadoWorkspace.Dy[41] -= acadoVariables.y[41];
-acadoWorkspace.Dy[42] -= acadoVariables.y[42];
-acadoWorkspace.Dy[43] -= acadoVariables.y[43];
-acadoWorkspace.Dy[44] -= acadoVariables.y[44];
-acadoWorkspace.Dy[45] -= acadoVariables.y[45];
-acadoWorkspace.Dy[46] -= acadoVariables.y[46];
-acadoWorkspace.Dy[47] -= acadoVariables.y[47];
-acadoWorkspace.Dy[48] -= acadoVariables.y[48];
-acadoWorkspace.Dy[49] -= acadoVariables.y[49];
-acadoWorkspace.Dy[50] -= acadoVariables.y[50];
-acadoWorkspace.Dy[51] -= acadoVariables.y[51];
-acadoWorkspace.Dy[52] -= acadoVariables.y[52];
-acadoWorkspace.Dy[53] -= acadoVariables.y[53];
-acadoWorkspace.Dy[54] -= acadoVariables.y[54];
-acadoWorkspace.Dy[55] -= acadoVariables.y[55];
-acadoWorkspace.Dy[56] -= acadoVariables.y[56];
-acadoWorkspace.Dy[57] -= acadoVariables.y[57];
-acadoWorkspace.Dy[58] -= acadoVariables.y[58];
-acadoWorkspace.Dy[59] -= acadoVariables.y[59];
-acadoWorkspace.Dy[60] -= acadoVariables.y[60];
-acadoWorkspace.Dy[61] -= acadoVariables.y[61];
-acadoWorkspace.Dy[62] -= acadoVariables.y[62];
-acadoWorkspace.Dy[63] -= acadoVariables.y[63];
-acadoWorkspace.Dy[64] -= acadoVariables.y[64];
-acadoWorkspace.Dy[65] -= acadoVariables.y[65];
-acadoWorkspace.Dy[66] -= acadoVariables.y[66];
-acadoWorkspace.Dy[67] -= acadoVariables.y[67];
-acadoWorkspace.Dy[68] -= acadoVariables.y[68];
-acadoWorkspace.Dy[69] -= acadoVariables.y[69];
-acadoWorkspace.Dy[70] -= acadoVariables.y[70];
-acadoWorkspace.Dy[71] -= acadoVariables.y[71];
-acadoWorkspace.Dy[72] -= acadoVariables.y[72];
-acadoWorkspace.Dy[73] -= acadoVariables.y[73];
-acadoWorkspace.Dy[74] -= acadoVariables.y[74];
-acadoWorkspace.Dy[75] -= acadoVariables.y[75];
-acadoWorkspace.Dy[76] -= acadoVariables.y[76];
-acadoWorkspace.Dy[77] -= acadoVariables.y[77];
-acadoWorkspace.Dy[78] -= acadoVariables.y[78];
-acadoWorkspace.Dy[79] -= acadoVariables.y[79];
-acadoWorkspace.DyN[0] -= acadoVariables.yN[0];
-acadoWorkspace.DyN[1] -= acadoVariables.yN[1];
-acadoWorkspace.DyN[2] -= acadoVariables.yN[2];
-
-acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 3 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 4 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 8 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 16 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 28 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 32 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 44 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 48 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 52 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 56 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 64 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 68 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 72 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 76 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.g[ 22 ]) );
-
-acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy );
-acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.QDy[ 3 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.QDy[ 6 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 9 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.QDy[ 12 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 15 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 18 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.QDy[ 21 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.QDy[ 24 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 27 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 30 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.QDy[ 33 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.QDy[ 36 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.QDy[ 39 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.QDy[ 42 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 45 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 192 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.QDy[ 48 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 204 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.QDy[ 51 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 216 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.QDy[ 54 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 228 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.QDy[ 57 ]) );
-
-acadoWorkspace.QDy[60] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[61] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[62] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2];
-
-acadoWorkspace.QDy[3] += acadoWorkspace.Qd[0];
-acadoWorkspace.QDy[4] += acadoWorkspace.Qd[1];
-acadoWorkspace.QDy[5] += acadoWorkspace.Qd[2];
-acadoWorkspace.QDy[6] += acadoWorkspace.Qd[3];
-acadoWorkspace.QDy[7] += acadoWorkspace.Qd[4];
-acadoWorkspace.QDy[8] += acadoWorkspace.Qd[5];
-acadoWorkspace.QDy[9] += acadoWorkspace.Qd[6];
-acadoWorkspace.QDy[10] += acadoWorkspace.Qd[7];
-acadoWorkspace.QDy[11] += acadoWorkspace.Qd[8];
-acadoWorkspace.QDy[12] += acadoWorkspace.Qd[9];
-acadoWorkspace.QDy[13] += acadoWorkspace.Qd[10];
-acadoWorkspace.QDy[14] += acadoWorkspace.Qd[11];
-acadoWorkspace.QDy[15] += acadoWorkspace.Qd[12];
-acadoWorkspace.QDy[16] += acadoWorkspace.Qd[13];
-acadoWorkspace.QDy[17] += acadoWorkspace.Qd[14];
-acadoWorkspace.QDy[18] += acadoWorkspace.Qd[15];
-acadoWorkspace.QDy[19] += acadoWorkspace.Qd[16];
-acadoWorkspace.QDy[20] += acadoWorkspace.Qd[17];
-acadoWorkspace.QDy[21] += acadoWorkspace.Qd[18];
-acadoWorkspace.QDy[22] += acadoWorkspace.Qd[19];
-acadoWorkspace.QDy[23] += acadoWorkspace.Qd[20];
-acadoWorkspace.QDy[24] += acadoWorkspace.Qd[21];
-acadoWorkspace.QDy[25] += acadoWorkspace.Qd[22];
-acadoWorkspace.QDy[26] += acadoWorkspace.Qd[23];
-acadoWorkspace.QDy[27] += acadoWorkspace.Qd[24];
-acadoWorkspace.QDy[28] += acadoWorkspace.Qd[25];
-acadoWorkspace.QDy[29] += acadoWorkspace.Qd[26];
-acadoWorkspace.QDy[30] += acadoWorkspace.Qd[27];
-acadoWorkspace.QDy[31] += acadoWorkspace.Qd[28];
-acadoWorkspace.QDy[32] += acadoWorkspace.Qd[29];
-acadoWorkspace.QDy[33] += acadoWorkspace.Qd[30];
-acadoWorkspace.QDy[34] += acadoWorkspace.Qd[31];
-acadoWorkspace.QDy[35] += acadoWorkspace.Qd[32];
-acadoWorkspace.QDy[36] += acadoWorkspace.Qd[33];
-acadoWorkspace.QDy[37] += acadoWorkspace.Qd[34];
-acadoWorkspace.QDy[38] += acadoWorkspace.Qd[35];
-acadoWorkspace.QDy[39] += acadoWorkspace.Qd[36];
-acadoWorkspace.QDy[40] += acadoWorkspace.Qd[37];
-acadoWorkspace.QDy[41] += acadoWorkspace.Qd[38];
-acadoWorkspace.QDy[42] += acadoWorkspace.Qd[39];
-acadoWorkspace.QDy[43] += acadoWorkspace.Qd[40];
-acadoWorkspace.QDy[44] += acadoWorkspace.Qd[41];
-acadoWorkspace.QDy[45] += acadoWorkspace.Qd[42];
-acadoWorkspace.QDy[46] += acadoWorkspace.Qd[43];
-acadoWorkspace.QDy[47] += acadoWorkspace.Qd[44];
-acadoWorkspace.QDy[48] += acadoWorkspace.Qd[45];
-acadoWorkspace.QDy[49] += acadoWorkspace.Qd[46];
-acadoWorkspace.QDy[50] += acadoWorkspace.Qd[47];
-acadoWorkspace.QDy[51] += acadoWorkspace.Qd[48];
-acadoWorkspace.QDy[52] += acadoWorkspace.Qd[49];
-acadoWorkspace.QDy[53] += acadoWorkspace.Qd[50];
-acadoWorkspace.QDy[54] += acadoWorkspace.Qd[51];
-acadoWorkspace.QDy[55] += acadoWorkspace.Qd[52];
-acadoWorkspace.QDy[56] += acadoWorkspace.Qd[53];
-acadoWorkspace.QDy[57] += acadoWorkspace.Qd[54];
-acadoWorkspace.QDy[58] += acadoWorkspace.Qd[55];
-acadoWorkspace.QDy[59] += acadoWorkspace.Qd[56];
-acadoWorkspace.QDy[60] += acadoWorkspace.Qd[57];
-acadoWorkspace.QDy[61] += acadoWorkspace.Qd[58];
-acadoWorkspace.QDy[62] += acadoWorkspace.Qd[59];
-
-acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[62];
-acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[62];
-acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[62];
-
-
-acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 3 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 3 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 22 ]) );
-
-acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2];
-tmp = acadoVariables.x[4] + acadoWorkspace.d[1];
-acadoWorkspace.lbA[0] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[0] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[7] + acadoWorkspace.d[4];
-acadoWorkspace.lbA[1] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[1] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[10] + acadoWorkspace.d[7];
-acadoWorkspace.lbA[2] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[2] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[13] + acadoWorkspace.d[10];
-acadoWorkspace.lbA[3] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[3] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[16] + acadoWorkspace.d[13];
-acadoWorkspace.lbA[4] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[4] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[19] + acadoWorkspace.d[16];
-acadoWorkspace.lbA[5] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[5] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[22] + acadoWorkspace.d[19];
-acadoWorkspace.lbA[6] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[6] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[25] + acadoWorkspace.d[22];
-acadoWorkspace.lbA[7] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[7] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[28] + acadoWorkspace.d[25];
-acadoWorkspace.lbA[8] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[8] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[31] + acadoWorkspace.d[28];
-acadoWorkspace.lbA[9] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[9] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[34] + acadoWorkspace.d[31];
-acadoWorkspace.lbA[10] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[10] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[37] + acadoWorkspace.d[34];
-acadoWorkspace.lbA[11] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[11] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[40] + acadoWorkspace.d[37];
-acadoWorkspace.lbA[12] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[12] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[43] + acadoWorkspace.d[40];
-acadoWorkspace.lbA[13] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[13] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[46] + acadoWorkspace.d[43];
-acadoWorkspace.lbA[14] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[14] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[49] + acadoWorkspace.d[46];
-acadoWorkspace.lbA[15] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[15] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[52] + acadoWorkspace.d[49];
-acadoWorkspace.lbA[16] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[16] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[55] + acadoWorkspace.d[52];
-acadoWorkspace.lbA[17] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[17] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[58] + acadoWorkspace.d[55];
-acadoWorkspace.lbA[18] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[61] + acadoWorkspace.d[58];
-acadoWorkspace.lbA[19] = (real_t)-1.0000000000000001e-01 - tmp;
-acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp;
-
-}
-
-void acado_expand(  )
-{
-acadoVariables.x[0] += acadoWorkspace.x[0];
-acadoVariables.x[1] += acadoWorkspace.x[1];
-acadoVariables.x[2] += acadoWorkspace.x[2];
-
-acadoVariables.u[0] += acadoWorkspace.x[3];
-acadoVariables.u[1] += acadoWorkspace.x[4];
-acadoVariables.u[2] += acadoWorkspace.x[5];
-acadoVariables.u[3] += acadoWorkspace.x[6];
-acadoVariables.u[4] += acadoWorkspace.x[7];
-acadoVariables.u[5] += acadoWorkspace.x[8];
-acadoVariables.u[6] += acadoWorkspace.x[9];
-acadoVariables.u[7] += acadoWorkspace.x[10];
-acadoVariables.u[8] += acadoWorkspace.x[11];
-acadoVariables.u[9] += acadoWorkspace.x[12];
-acadoVariables.u[10] += acadoWorkspace.x[13];
-acadoVariables.u[11] += acadoWorkspace.x[14];
-acadoVariables.u[12] += acadoWorkspace.x[15];
-acadoVariables.u[13] += acadoWorkspace.x[16];
-acadoVariables.u[14] += acadoWorkspace.x[17];
-acadoVariables.u[15] += acadoWorkspace.x[18];
-acadoVariables.u[16] += acadoWorkspace.x[19];
-acadoVariables.u[17] += acadoWorkspace.x[20];
-acadoVariables.u[18] += acadoWorkspace.x[21];
-acadoVariables.u[19] += acadoWorkspace.x[22];
-
-acadoVariables.x[3] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.d[0];
-acadoVariables.x[4] += + acadoWorkspace.evGx[3]*acadoWorkspace.x[0] + acadoWorkspace.evGx[4]*acadoWorkspace.x[1] + acadoWorkspace.evGx[5]*acadoWorkspace.x[2] + acadoWorkspace.d[1];
-acadoVariables.x[5] += + acadoWorkspace.evGx[6]*acadoWorkspace.x[0] + acadoWorkspace.evGx[7]*acadoWorkspace.x[1] + acadoWorkspace.evGx[8]*acadoWorkspace.x[2] + acadoWorkspace.d[2];
-acadoVariables.x[6] += + acadoWorkspace.evGx[9]*acadoWorkspace.x[0] + acadoWorkspace.evGx[10]*acadoWorkspace.x[1] + acadoWorkspace.evGx[11]*acadoWorkspace.x[2] + acadoWorkspace.d[3];
-acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.d[4];
-acadoVariables.x[8] += + acadoWorkspace.evGx[15]*acadoWorkspace.x[0] + acadoWorkspace.evGx[16]*acadoWorkspace.x[1] + acadoWorkspace.evGx[17]*acadoWorkspace.x[2] + acadoWorkspace.d[5];
-acadoVariables.x[9] += + acadoWorkspace.evGx[18]*acadoWorkspace.x[0] + acadoWorkspace.evGx[19]*acadoWorkspace.x[1] + acadoWorkspace.evGx[20]*acadoWorkspace.x[2] + acadoWorkspace.d[6];
-acadoVariables.x[10] += + acadoWorkspace.evGx[21]*acadoWorkspace.x[0] + acadoWorkspace.evGx[22]*acadoWorkspace.x[1] + acadoWorkspace.evGx[23]*acadoWorkspace.x[2] + acadoWorkspace.d[7];
-acadoVariables.x[11] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.d[8];
-acadoVariables.x[12] += + acadoWorkspace.evGx[27]*acadoWorkspace.x[0] + acadoWorkspace.evGx[28]*acadoWorkspace.x[1] + acadoWorkspace.evGx[29]*acadoWorkspace.x[2] + acadoWorkspace.d[9];
-acadoVariables.x[13] += + acadoWorkspace.evGx[30]*acadoWorkspace.x[0] + acadoWorkspace.evGx[31]*acadoWorkspace.x[1] + acadoWorkspace.evGx[32]*acadoWorkspace.x[2] + acadoWorkspace.d[10];
-acadoVariables.x[14] += + acadoWorkspace.evGx[33]*acadoWorkspace.x[0] + acadoWorkspace.evGx[34]*acadoWorkspace.x[1] + acadoWorkspace.evGx[35]*acadoWorkspace.x[2] + acadoWorkspace.d[11];
-acadoVariables.x[15] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.d[12];
-acadoVariables.x[16] += + acadoWorkspace.evGx[39]*acadoWorkspace.x[0] + acadoWorkspace.evGx[40]*acadoWorkspace.x[1] + acadoWorkspace.evGx[41]*acadoWorkspace.x[2] + acadoWorkspace.d[13];
-acadoVariables.x[17] += + acadoWorkspace.evGx[42]*acadoWorkspace.x[0] + acadoWorkspace.evGx[43]*acadoWorkspace.x[1] + acadoWorkspace.evGx[44]*acadoWorkspace.x[2] + acadoWorkspace.d[14];
-acadoVariables.x[18] += + acadoWorkspace.evGx[45]*acadoWorkspace.x[0] + acadoWorkspace.evGx[46]*acadoWorkspace.x[1] + acadoWorkspace.evGx[47]*acadoWorkspace.x[2] + acadoWorkspace.d[15];
-acadoVariables.x[19] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.d[16];
-acadoVariables.x[20] += + acadoWorkspace.evGx[51]*acadoWorkspace.x[0] + acadoWorkspace.evGx[52]*acadoWorkspace.x[1] + acadoWorkspace.evGx[53]*acadoWorkspace.x[2] + acadoWorkspace.d[17];
-acadoVariables.x[21] += + acadoWorkspace.evGx[54]*acadoWorkspace.x[0] + acadoWorkspace.evGx[55]*acadoWorkspace.x[1] + acadoWorkspace.evGx[56]*acadoWorkspace.x[2] + acadoWorkspace.d[18];
-acadoVariables.x[22] += + acadoWorkspace.evGx[57]*acadoWorkspace.x[0] + acadoWorkspace.evGx[58]*acadoWorkspace.x[1] + acadoWorkspace.evGx[59]*acadoWorkspace.x[2] + acadoWorkspace.d[19];
-acadoVariables.x[23] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.d[20];
-acadoVariables.x[24] += + acadoWorkspace.evGx[63]*acadoWorkspace.x[0] + acadoWorkspace.evGx[64]*acadoWorkspace.x[1] + acadoWorkspace.evGx[65]*acadoWorkspace.x[2] + acadoWorkspace.d[21];
-acadoVariables.x[25] += + acadoWorkspace.evGx[66]*acadoWorkspace.x[0] + acadoWorkspace.evGx[67]*acadoWorkspace.x[1] + acadoWorkspace.evGx[68]*acadoWorkspace.x[2] + acadoWorkspace.d[22];
-acadoVariables.x[26] += + acadoWorkspace.evGx[69]*acadoWorkspace.x[0] + acadoWorkspace.evGx[70]*acadoWorkspace.x[1] + acadoWorkspace.evGx[71]*acadoWorkspace.x[2] + acadoWorkspace.d[23];
-acadoVariables.x[27] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.d[24];
-acadoVariables.x[28] += + acadoWorkspace.evGx[75]*acadoWorkspace.x[0] + acadoWorkspace.evGx[76]*acadoWorkspace.x[1] + acadoWorkspace.evGx[77]*acadoWorkspace.x[2] + acadoWorkspace.d[25];
-acadoVariables.x[29] += + acadoWorkspace.evGx[78]*acadoWorkspace.x[0] + acadoWorkspace.evGx[79]*acadoWorkspace.x[1] + acadoWorkspace.evGx[80]*acadoWorkspace.x[2] + acadoWorkspace.d[26];
-acadoVariables.x[30] += + acadoWorkspace.evGx[81]*acadoWorkspace.x[0] + acadoWorkspace.evGx[82]*acadoWorkspace.x[1] + acadoWorkspace.evGx[83]*acadoWorkspace.x[2] + acadoWorkspace.d[27];
-acadoVariables.x[31] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.d[28];
-acadoVariables.x[32] += + acadoWorkspace.evGx[87]*acadoWorkspace.x[0] + acadoWorkspace.evGx[88]*acadoWorkspace.x[1] + acadoWorkspace.evGx[89]*acadoWorkspace.x[2] + acadoWorkspace.d[29];
-acadoVariables.x[33] += + acadoWorkspace.evGx[90]*acadoWorkspace.x[0] + acadoWorkspace.evGx[91]*acadoWorkspace.x[1] + acadoWorkspace.evGx[92]*acadoWorkspace.x[2] + acadoWorkspace.d[30];
-acadoVariables.x[34] += + acadoWorkspace.evGx[93]*acadoWorkspace.x[0] + acadoWorkspace.evGx[94]*acadoWorkspace.x[1] + acadoWorkspace.evGx[95]*acadoWorkspace.x[2] + acadoWorkspace.d[31];
-acadoVariables.x[35] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.d[32];
-acadoVariables.x[36] += + acadoWorkspace.evGx[99]*acadoWorkspace.x[0] + acadoWorkspace.evGx[100]*acadoWorkspace.x[1] + acadoWorkspace.evGx[101]*acadoWorkspace.x[2] + acadoWorkspace.d[33];
-acadoVariables.x[37] += + acadoWorkspace.evGx[102]*acadoWorkspace.x[0] + acadoWorkspace.evGx[103]*acadoWorkspace.x[1] + acadoWorkspace.evGx[104]*acadoWorkspace.x[2] + acadoWorkspace.d[34];
-acadoVariables.x[38] += + acadoWorkspace.evGx[105]*acadoWorkspace.x[0] + acadoWorkspace.evGx[106]*acadoWorkspace.x[1] + acadoWorkspace.evGx[107]*acadoWorkspace.x[2] + acadoWorkspace.d[35];
-acadoVariables.x[39] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.d[36];
-acadoVariables.x[40] += + acadoWorkspace.evGx[111]*acadoWorkspace.x[0] + acadoWorkspace.evGx[112]*acadoWorkspace.x[1] + acadoWorkspace.evGx[113]*acadoWorkspace.x[2] + acadoWorkspace.d[37];
-acadoVariables.x[41] += + acadoWorkspace.evGx[114]*acadoWorkspace.x[0] + acadoWorkspace.evGx[115]*acadoWorkspace.x[1] + acadoWorkspace.evGx[116]*acadoWorkspace.x[2] + acadoWorkspace.d[38];
-acadoVariables.x[42] += + acadoWorkspace.evGx[117]*acadoWorkspace.x[0] + acadoWorkspace.evGx[118]*acadoWorkspace.x[1] + acadoWorkspace.evGx[119]*acadoWorkspace.x[2] + acadoWorkspace.d[39];
-acadoVariables.x[43] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.d[40];
-acadoVariables.x[44] += + acadoWorkspace.evGx[123]*acadoWorkspace.x[0] + acadoWorkspace.evGx[124]*acadoWorkspace.x[1] + acadoWorkspace.evGx[125]*acadoWorkspace.x[2] + acadoWorkspace.d[41];
-acadoVariables.x[45] += + acadoWorkspace.evGx[126]*acadoWorkspace.x[0] + acadoWorkspace.evGx[127]*acadoWorkspace.x[1] + acadoWorkspace.evGx[128]*acadoWorkspace.x[2] + acadoWorkspace.d[42];
-acadoVariables.x[46] += + acadoWorkspace.evGx[129]*acadoWorkspace.x[0] + acadoWorkspace.evGx[130]*acadoWorkspace.x[1] + acadoWorkspace.evGx[131]*acadoWorkspace.x[2] + acadoWorkspace.d[43];
-acadoVariables.x[47] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.d[44];
-acadoVariables.x[48] += + acadoWorkspace.evGx[135]*acadoWorkspace.x[0] + acadoWorkspace.evGx[136]*acadoWorkspace.x[1] + acadoWorkspace.evGx[137]*acadoWorkspace.x[2] + acadoWorkspace.d[45];
-acadoVariables.x[49] += + acadoWorkspace.evGx[138]*acadoWorkspace.x[0] + acadoWorkspace.evGx[139]*acadoWorkspace.x[1] + acadoWorkspace.evGx[140]*acadoWorkspace.x[2] + acadoWorkspace.d[46];
-acadoVariables.x[50] += + acadoWorkspace.evGx[141]*acadoWorkspace.x[0] + acadoWorkspace.evGx[142]*acadoWorkspace.x[1] + acadoWorkspace.evGx[143]*acadoWorkspace.x[2] + acadoWorkspace.d[47];
-acadoVariables.x[51] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.d[48];
-acadoVariables.x[52] += + acadoWorkspace.evGx[147]*acadoWorkspace.x[0] + acadoWorkspace.evGx[148]*acadoWorkspace.x[1] + acadoWorkspace.evGx[149]*acadoWorkspace.x[2] + acadoWorkspace.d[49];
-acadoVariables.x[53] += + acadoWorkspace.evGx[150]*acadoWorkspace.x[0] + acadoWorkspace.evGx[151]*acadoWorkspace.x[1] + acadoWorkspace.evGx[152]*acadoWorkspace.x[2] + acadoWorkspace.d[50];
-acadoVariables.x[54] += + acadoWorkspace.evGx[153]*acadoWorkspace.x[0] + acadoWorkspace.evGx[154]*acadoWorkspace.x[1] + acadoWorkspace.evGx[155]*acadoWorkspace.x[2] + acadoWorkspace.d[51];
-acadoVariables.x[55] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.d[52];
-acadoVariables.x[56] += + acadoWorkspace.evGx[159]*acadoWorkspace.x[0] + acadoWorkspace.evGx[160]*acadoWorkspace.x[1] + acadoWorkspace.evGx[161]*acadoWorkspace.x[2] + acadoWorkspace.d[53];
-acadoVariables.x[57] += + acadoWorkspace.evGx[162]*acadoWorkspace.x[0] + acadoWorkspace.evGx[163]*acadoWorkspace.x[1] + acadoWorkspace.evGx[164]*acadoWorkspace.x[2] + acadoWorkspace.d[54];
-acadoVariables.x[58] += + acadoWorkspace.evGx[165]*acadoWorkspace.x[0] + acadoWorkspace.evGx[166]*acadoWorkspace.x[1] + acadoWorkspace.evGx[167]*acadoWorkspace.x[2] + acadoWorkspace.d[55];
-acadoVariables.x[59] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.d[56];
-acadoVariables.x[60] += + acadoWorkspace.evGx[171]*acadoWorkspace.x[0] + acadoWorkspace.evGx[172]*acadoWorkspace.x[1] + acadoWorkspace.evGx[173]*acadoWorkspace.x[2] + acadoWorkspace.d[57];
-acadoVariables.x[61] += + acadoWorkspace.evGx[174]*acadoWorkspace.x[0] + acadoWorkspace.evGx[175]*acadoWorkspace.x[1] + acadoWorkspace.evGx[176]*acadoWorkspace.x[2] + acadoWorkspace.d[58];
-acadoVariables.x[62] += + acadoWorkspace.evGx[177]*acadoWorkspace.x[0] + acadoWorkspace.evGx[178]*acadoWorkspace.x[1] + acadoWorkspace.evGx[179]*acadoWorkspace.x[2] + acadoWorkspace.d[59];
-
-acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 3 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 6 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 6 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 9 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 9 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 9 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 15 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 15 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 15 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 15 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 15 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 21 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 27 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 33 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 39 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 45 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 51 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 57 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) );
-}
-
-int acado_preparationStep(  )
-{
-int ret;
-
-ret = acado_modelSimulation();
-acado_evaluateObjective(  );
-acado_condensePrep(  );
-return ret;
-}
-
-int acado_feedbackStep(  )
-{
-int tmp;
-
-acado_condenseFdb(  );
-
-tmp = acado_solve( );
-
-acado_expand(  );
-return tmp;
-}
-
-int acado_initializeSolver(  )
-{
-int ret;
-
-/* This is a function which must be called once before any other function call! */
-
-
-ret = 0;
-
-memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
-return ret;
-}
-
-void acado_initializeNodesByForwardSimulation(  )
-{
-int index;
-for (index = 0; index < 20; ++index)
-{
-acadoWorkspace.state[0] = acadoVariables.x[index * 3];
-acadoWorkspace.state[1] = acadoVariables.x[index * 3 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[index * 3 + 2];
-acadoWorkspace.state[15] = acadoVariables.u[index];
-acadoWorkspace.state[16] = acadoVariables.od[index * 2];
-acadoWorkspace.state[17] = acadoVariables.od[index * 2 + 1];
-
-acado_integrate(acadoWorkspace.state, index == 0, index);
-
-acadoVariables.x[index * 3 + 3] = acadoWorkspace.state[0];
-acadoVariables.x[index * 3 + 4] = acadoWorkspace.state[1];
-acadoVariables.x[index * 3 + 5] = acadoWorkspace.state[2];
-}
-}
-
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 20; ++index)
-{
-acadoVariables.x[index * 3] = acadoVariables.x[index * 3 + 3];
-acadoVariables.x[index * 3 + 1] = acadoVariables.x[index * 3 + 4];
-acadoVariables.x[index * 3 + 2] = acadoVariables.x[index * 3 + 5];
-}
-
-if (strategy == 1 && xEnd != 0)
-{
-acadoVariables.x[60] = xEnd[0];
-acadoVariables.x[61] = xEnd[1];
-acadoVariables.x[62] = xEnd[2];
-}
-else if (strategy == 2) 
-{
-acadoWorkspace.state[0] = acadoVariables.x[60];
-acadoWorkspace.state[1] = acadoVariables.x[61];
-acadoWorkspace.state[2] = acadoVariables.x[62];
-if (uEnd != 0)
-{
-acadoWorkspace.state[15] = uEnd[0];
-}
-else
-{
-acadoWorkspace.state[15] = acadoVariables.u[19];
-}
-acadoWorkspace.state[16] = acadoVariables.od[40];
-acadoWorkspace.state[17] = acadoVariables.od[41];
-
-acado_integrate(acadoWorkspace.state, 1, 19);
-
-acadoVariables.x[60] = acadoWorkspace.state[0];
-acadoVariables.x[61] = acadoWorkspace.state[1];
-acadoVariables.x[62] = acadoWorkspace.state[2];
-}
-}
-
-void acado_shiftControls( real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 19; ++index)
-{
-acadoVariables.u[index] = acadoVariables.u[index + 1];
-}
-
-if (uEnd != 0)
-{
-acadoVariables.u[19] = uEnd[0];
-}
-}
-
-real_t acado_getKKT(  )
-{
-real_t kkt;
-
-int index;
-real_t prd;
-
-kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22];
-kkt = fabs( kkt );
-for (index = 0; index < 23; ++index)
-{
-prd = acadoWorkspace.y[index];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lb[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ub[index] * prd);
-}
-for (index = 0; index < 20; ++index)
-{
-prd = acadoWorkspace.y[index + 23];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lbA[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ubA[index] * prd);
-}
-return kkt;
-}
-
-real_t acado_getObjective(  )
-{
-real_t objVal;
-
-int lRun1;
-/** Row vector of size: 4 */
-real_t tmpDy[ 4 ];
-
-/** Row vector of size: 3 */
-real_t tmpDyN[ 3 ];
-
-for (lRun1 = 0; lRun1 < 20; ++lRun1)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 3];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 3 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 3 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4];
-acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1];
-acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2];
-acadoWorkspace.Dy[lRun1 * 4 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 4 + 3];
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[60];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
-acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
-acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2];
-objVal = 0.0000000000000000e+00;
-for (lRun1 = 0; lRun1 < 20; ++lRun1)
-{
-tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 4] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 8] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 12];
-tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 1] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 5] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 9] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 13];
-tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 2] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 6] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 10] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 14];
-tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 4]*acadoVariables.W[lRun1 * 16 + 3] + acadoWorkspace.Dy[lRun1 * 4 + 1]*acadoVariables.W[lRun1 * 16 + 7] + acadoWorkspace.Dy[lRun1 * 4 + 2]*acadoVariables.W[lRun1 * 16 + 11] + acadoWorkspace.Dy[lRun1 * 4 + 3]*acadoVariables.W[lRun1 * 16 + 15];
-objVal += + acadoWorkspace.Dy[lRun1 * 4]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 4 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 4 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 4 + 3]*tmpDy[3];
-}
-
-tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[3] + acadoWorkspace.DyN[2]*acadoVariables.WN[6];
-tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[7];
-tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[8];
-objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2];
-
-objVal *= 0.5;
-return objVal;
-}
-
diff --git a/selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py b/selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py
deleted file mode 100644
index d6d11be01..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/libmpc_py.py
+++ /dev/null
@@ -1,41 +0,0 @@
-import os
-
-from cffi import FFI
-from common.ffi_wrapper import suffix
-
-mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-
-def _get_libmpc(mpc_id):
-    libmpc_fn = os.path.join(mpc_dir, "libmpc%d%s" % (mpc_id, suffix()))
-
-    ffi = FFI()
-    ffi.cdef("""
-    typedef struct {
-    double x_ego, v_ego, a_ego, x_l, v_l, a_l;
-    } state_t;
-
-
-    typedef struct {
-    double x_ego[21];
-    double v_ego[21];
-    double a_ego[21];
-    double j_ego[20];
-    double x_l[21];
-    double v_l[21];
-    double a_l[21];
-    double t[21];
-    double cost;
-    } log_t;
-
-    void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost);
-    void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l);
-    int run_mpc(state_t * x0, log_t * solution,
-                double l, double a_l_0);
-    """)
-
-    return (ffi, ffi.dlopen(libmpc_fn))
-
-mpcs = [_get_libmpc(0), _get_libmpc(1)]
-
-def get_libmpc(mpc_id):
-    return mpcs[mpc_id]
diff --git a/selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c b/selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c
deleted file mode 100644
index d4bfee8c8..000000000
--- a/selfdrive/controls/lib/lead_mpc_lib/longitudinal_mpc.c
+++ /dev/null
@@ -1,173 +0,0 @@
-#include "acado_common.h"
-#include "acado_auxiliary_functions.h"
-
-#include <stdio.h>
-#include <math.h>
-
-#define NX          ACADO_NX  /* Number of differential state variables.  */
-#define NXA         ACADO_NXA /* Number of algebraic variables. */
-#define NU          ACADO_NU  /* Number of control inputs. */
-#define NOD         ACADO_NOD  /* Number of online data values. */
-
-#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
-#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
-
-#define N           ACADO_N   /* Number of intervals in the horizon. */
-
-ACADOvariables acadoVariables;
-ACADOworkspace acadoWorkspace;
-
-typedef struct {
-  double x_ego, v_ego, a_ego, x_l, v_l, a_l;
-} state_t;
-
-
-typedef struct {
-  double x_ego[N+1];
-  double v_ego[N+1];
-  double a_ego[N+1];
-  double j_ego[N];
-  double x_l[N+1];
-  double v_l[N+1];
-  double a_l[N+1];
-  double t[N+1];
-  double cost;
-} log_t;
-
-void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){
-  acado_initializeSolver();
-  int    i;
-  const int STEP_MULTIPLIER = 3;
-
-  /* Initialize the states and controls. */
-  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
-  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
-
-  /* Initialize the measurements/reference. */
-  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
-  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
-
-  /* MPC: initialize the current state feedback. */
-  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
-  // Set weights
-
-  for (i = 0; i < N; i++) {
-    int f = 1;
-    if (i > 4){
-      f = STEP_MULTIPLIER;
-    }
-    // Setup diagonal entries
-    acadoVariables.W[NY*NY*i + (NY+1)*0] = ttcCost * f; // exponential cost for time-to-collision (ttc)
-    acadoVariables.W[NY*NY*i + (NY+1)*1] = distanceCost * f; // desired distance
-    acadoVariables.W[NY*NY*i + (NY+1)*2] = accelerationCost * f; // acceleration
-    acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f; // jerk
-  }
-  acadoVariables.WN[(NYN+1)*0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone
-  acadoVariables.WN[(NYN+1)*1] = distanceCost * STEP_MULTIPLIER; // desired distance
-  acadoVariables.WN[(NYN+1)*2] = accelerationCost * STEP_MULTIPLIER; // acceleration
-
-}
-
-void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
-  int i;
-
-  double x_l = x_l_0;
-  double v_l = v_l_0;
-  double a_l = a_l_0;
-
-  double x_ego = 0.0;
-  double a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
-
-  if (a_ego > 0){
-    a_ego = 0.0;
-  }
-
-
-  double dt = 0.2;
-  double t = 0.;
-
-  for (i = 0; i < N + 1; ++i){
-    if (i > 4){
-      dt = 0.6;
-    }
-
-    /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_ego, v_ego, a_l); */
-    acadoVariables.x[i*NX] = x_ego;
-    acadoVariables.x[i*NX+1] = v_ego;
-    acadoVariables.x[i*NX+2] = a_ego;
-
-    v_ego += a_ego * dt;
-
-    if (v_ego <= 0.0) {
-      v_ego = 0.0;
-      a_ego = 0.0;
-    }
-
-    x_ego += v_ego * dt;
-    t += dt;
-  }
-
-  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
-  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
-  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
-}
-
-int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
-  // Calculate lead vehicle predictions
-  int i;
-  double t = 0.;
-  double dt = 0.2;
-  double x_l = x0->x_l;
-  double v_l = x0->v_l;
-  double a_l = a_l_0;
-
-  /* printf("t\tx_l\t_v_l\t_al\n"); */
-  for (i = 0; i < N + 1; ++i){
-    if (i > 4){
-      dt = 0.6;
-    }
-
-    /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_l, v_l, a_l); */
-
-    acadoVariables.od[i*NOD] = x_l;
-    acadoVariables.od[i*NOD+1] = v_l;
-
-    solution->x_l[i] = x_l;
-    solution->v_l[i] = v_l;
-    solution->a_l[i] = a_l;
-    solution->t[i] = t;
-
-    a_l = a_l_0 * exp(-l * t * t / 2);
-    x_l += v_l * dt;
-    v_l += a_l * dt;
-    if (v_l < 0.0){
-      a_l = 0.0;
-      v_l = 0.0;
-    }
-
-    t += dt;
-  }
-
-  acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
-  acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
-  acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
-
-  acado_preparationStep();
-  acado_feedbackStep();
-
-  for (i = 0; i <= N; i++){
-    solution->x_ego[i] = acadoVariables.x[i*NX];
-    solution->v_ego[i] = acadoVariables.x[i*NX+1];
-    solution->a_ego[i] = acadoVariables.x[i*NX+2];
-
-    if (i < N){
-      solution->j_ego[i] = acadoVariables.u[i];
-    }
-  }
-  solution->cost = acado_getObjective();
-
-  // Dont shift states here. Current solution is closer to next timestep than if
-  // we shift by 0.2 seconds.
-
-  return acado_getNWSR();
-}
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
deleted file mode 100644
index 4d6f3846d..000000000
--- a/selfdrive/controls/lib/long_mpc.py
+++ /dev/null
@@ -1,73 +0,0 @@
-import numpy as np
-import math
-
-from selfdrive.swaglog import cloudlog
-from common.realtime import sec_since_boot
-from selfdrive.controls.lib.longitudinal_mpc_lib import libmpc_py
-from selfdrive.controls.lib.drive_helpers import LON_MPC_N
-from selfdrive.modeld.constants import T_IDXS
-
-
-class LongitudinalMpc():
-  def __init__(self):
-    self.reset_mpc()
-    self.last_cloudlog_t = 0.0
-    self.ts = list(range(10))
-    self.status = True
-    self.min_a = -1.2
-    self.max_a = 1.2
-
-
-  def reset_mpc(self):
-    self.libmpc = libmpc_py.libmpc
-    self.libmpc.init(0.0, 1.0, 0.0, 50.0, 10000.0)
-
-    self.mpc_solution = libmpc_py.ffi.new("log_t *")
-    self.cur_state = libmpc_py.ffi.new("state_t *")
-
-    self.cur_state[0].x_ego = 0
-    self.cur_state[0].v_ego = 0
-    self.cur_state[0].a_ego = 0
-
-    self.v_solution = [0.0 for i in range(len(T_IDXS))]
-    self.a_solution = [0.0 for i in range(len(T_IDXS))]
-    self.j_solution = [0.0 for i in range(len(T_IDXS)-1)]
-
-  def set_accel_limits(self, min_a, max_a):
-    self.min_a = min_a
-    self.max_a = max_a
-
-  def set_cur_state(self, v, a):
-    v_safe = max(v, 1e-2)
-    a_safe = min(a, self.max_a - 1e-2)
-    a_safe = max(a_safe, self.min_a + 1e-2)
-    self.cur_state[0].x_ego = 0.0
-    self.cur_state[0].v_ego = v_safe
-    self.cur_state[0].a_ego = a_safe
-
-  def update(self, carstate, radarstate, v_cruise):
-    v_cruise_clipped = np.clip(v_cruise, self.cur_state[0].v_ego - 10., self.cur_state[0].v_ego + 10.0)
-    poss = v_cruise_clipped * np.array(T_IDXS[:LON_MPC_N+1])
-    speeds = v_cruise_clipped * np.ones(LON_MPC_N+1)
-    accels = np.zeros(LON_MPC_N+1)
-    self.update_with_xva(poss, speeds, accels)
-
-  def update_with_xva(self, poss, speeds, accels):
-    # Calculate mpc
-    self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
-                        list(poss), list(speeds), list(accels),
-                        self.min_a, self.max_a)
-
-    self.v_solution = list(self.mpc_solution.v_ego)
-    self.a_solution = list(self.mpc_solution.a_ego)
-    self.j_solution = list(self.mpc_solution.j_ego)
-
-    # Reset if NaN or goes through lead car
-    nans = any(math.isnan(x) for x in self.mpc_solution[0].v_ego)
-
-    t = sec_since_boot()
-    if nans:
-      if t > self.last_cloudlog_t + 5.0:
-        self.last_cloudlog_t = t
-        cloudlog.warning("Longitudinal model mpc reset - nans")
-      self.reset_mpc()
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index 28cd42b10..d44196e73 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -1,36 +1,29 @@
 from cereal import car
 from common.numpy_fast import clip, interp
+from common.realtime import DT_CTRL
 from selfdrive.controls.lib.pid import PIController
 from selfdrive.controls.lib.drive_helpers import CONTROL_N
 from selfdrive.modeld.constants import T_IDXS
 
 LongCtrlState = car.CarControl.Actuators.LongControlState
 
-STOPPING_EGO_SPEED = 0.5
 STOPPING_TARGET_SPEED_OFFSET = 0.01
-STARTING_TARGET_SPEED = 0.5
-DECEL_THRESHOLD_TO_PID = 0.8
-
-DECEL_STOPPING_TARGET = 2.0  # apply at least this amount of brake to maintain the vehicle stationary
-
-RATE = 100.0
 
 # As per ISO 15622:2018 for all speeds
 ACCEL_MIN_ISO = -3.5 # m/s^2
 ACCEL_MAX_ISO = 2.0 # m/s^2
 
 
-# TODO this logic isn't really car independent, does not belong here
-def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_pid,
                              output_accel, brake_pressed, cruise_standstill, min_speed_can):
   """Update longitudinal control state machine"""
   stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
   stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
-                       (v_ego < STOPPING_EGO_SPEED and
+                       (v_ego < CP.vEgoStopping and
                         ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or
                          brake_pressed))
 
-  starting_condition = v_target > STARTING_TARGET_SPEED and not cruise_standstill
+  starting_condition = v_target > CP.vEgoStarting and not cruise_standstill
 
   if not active:
     long_control_state = LongCtrlState.off
@@ -51,7 +44,7 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid,
     elif long_control_state == LongCtrlState.starting:
       if stopping_condition:
         long_control_state = LongCtrlState.stopping
-      elif output_accel >= -DECEL_THRESHOLD_TO_PID:
+      elif output_accel >= CP.startAccel:
         long_control_state = LongCtrlState.pid
 
   return long_control_state
@@ -62,7 +55,7 @@ class LongControl():
     self.long_control_state = LongCtrlState.off  # initialized to off
     self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV),
                             (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV),
-                            rate=RATE,
+                            rate=1/DT_CTRL,
                             sat_limit=0.8)
     self.v_pid = 0.0
     self.last_output_accel = 0.0
@@ -77,9 +70,16 @@ class LongControl():
     # Interp control trajectory
     # TODO estimate car specific lag, use .15s for now
     if len(long_plan.speeds) == CONTROL_N:
-      v_target = interp(CP.longitudinalActuatorDelay, T_IDXS[:CONTROL_N], long_plan.speeds)
+      v_target_lower = interp(CP.longitudinalActuatorDelayLowerBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_lower = 2 * (v_target_lower - long_plan.speeds[0])/CP.longitudinalActuatorDelayLowerBound - long_plan.accels[0]
+
+      v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
+      a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
+
+      v_target = min(v_target_lower, v_target_upper)
+      a_target = min(a_target_lower, a_target_upper)
+
       v_target_future = long_plan.speeds[-1]
-      a_target = 2 * (v_target - long_plan.speeds[0])/CP.longitudinalActuatorDelay - long_plan.accels[0]
     else:
       v_target = 0.0
       v_target_future = 0.0
@@ -93,7 +93,7 @@ class LongControl():
 
     # Update state machine
     output_accel = self.last_output_accel
-    self.long_control_state = long_control_state_trans(active, self.long_control_state, CS.vEgo,
+    self.long_control_state = long_control_state_trans(CP, active, self.long_control_state, CS.vEgo,
                                                        v_target_future, self.v_pid, output_accel,
                                                        CS.brakePressed, CS.cruiseState.standstill, CP.minSpeedCan)
 
@@ -121,19 +121,19 @@ class LongControl():
     # Intention is to stop, switch to a different brake control until we stop
     elif self.long_control_state == LongCtrlState.stopping:
       # Keep applying brakes until the car is stopped
-      if not CS.standstill or output_accel > -DECEL_STOPPING_TARGET:
-        output_accel -= CP.stoppingDecelRate / RATE
+      if not CS.standstill or output_accel > CP.stopAccel:
+        output_accel -= CP.stoppingDecelRate * DT_CTRL
       output_accel = clip(output_accel, accel_limits[0], accel_limits[1])
 
       self.reset(CS.vEgo)
 
     # Intention is to move again, release brake fast before handing control to PID
     elif self.long_control_state == LongCtrlState.starting:
-      if output_accel < -DECEL_THRESHOLD_TO_PID:
-        output_accel += CP.startingAccelRate / RATE
+      if output_accel < CP.startAccel:
+        output_accel += CP.startingAccelRate * DT_CTRL
       self.reset(CS.vEgo)
 
     self.last_output_accel = output_accel
     final_accel = clip(output_accel, accel_limits[0], accel_limits[1])
 
-    return final_accel, v_target, a_target
+    return final_accel
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore b/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
index f61a939cd..6f09b1aa0 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/.gitignore
@@ -1,2 +1,2 @@
-generator
-lib_qp/
+acados_ocp_long.json
+c_generated_code/
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
index 19387d4ae..70a3e72b1 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
@@ -1,47 +1,68 @@
 Import('env', 'arch')
 
-cpp_path = [
-    "#",
-    "#selfdrive",
-    "#phonelibs/acado/include",
-    "#phonelibs/acado/include/acado",
-    "#phonelibs/qpoases/INCLUDE",
-    "#phonelibs/qpoases/INCLUDE/EXTRAS",
-    "#phonelibs/qpoases/SRC/",
-    "#phonelibs/qpoases",
-    "lib_mpc_export",
+gen = "c_generated_code"
+
+casadi_model = [
+  f'{gen}/long_model/long_expl_ode_fun.c',
+  f'{gen}/long_model/long_expl_vde_forw.c',
 ]
 
-generated_c = [
-    'lib_mpc_export/acado_auxiliary_functions.c',
-    'lib_mpc_export/acado_qpoases_interface.cpp',
-    'lib_mpc_export/acado_integrator.c',
-    'lib_mpc_export/acado_solver.c',
+casadi_cost_y = [
+  f'{gen}/long_cost/long_cost_y_fun.c',
+  f'{gen}/long_cost/long_cost_y_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_hess.c',
 ]
 
-generated_h = [
-    'lib_mpc_export/acado_common.h',
-    'lib_mpc_export/acado_auxiliary_functions.h',
-    'lib_mpc_export/acado_qpoases_interface.hpp',
+casadi_cost_e = [
+  f'{gen}/long_cost/long_cost_y_e_fun.c',
+  f'{gen}/long_cost/long_cost_y_e_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_e_hess.c',
 ]
 
-interface_dir = Dir('lib_mpc_export')
+casadi_cost_0 = [
+  f'{gen}/long_cost/long_cost_y_0_fun.c',
+  f'{gen}/long_cost/long_cost_y_0_fun_jac_ut_xt.c',
+  f'{gen}/long_cost/long_cost_y_0_hess.c',
+]
 
-SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir'])
+casadi_constraints = [
+  f'{gen}/long_constraints/long_constr_h_fun.c',
+  f'{gen}/long_constraints/long_constr_h_fun_jac_uxt_zt.c',
+  f'{gen}/long_constraints/long_constr_h_e_fun.c',
+  f'{gen}/long_constraints/long_constr_h_e_fun_jac_uxt_zt.c',
+]
 
-if GetOption('mpc_generate'):
-  generator_cpp = File('generator.cpp')
+build_files = [f'{gen}/acados_solver_long.c'] + casadi_model + casadi_cost_y + casadi_cost_e +  \
+              casadi_cost_0 + casadi_constraints
 
-  acado_libs = [File(f"#phonelibs/acado/{arch}/lib/libacado_toolkit.a"),
-                File(f"#phonelibs/acado/{arch}/lib/libacado_casadi.a"),
-                File(f"#phonelibs/acado/{arch}/lib/libacado_csparse.a")]
+# extra generated files used to trigger a rebuild
+generated_files = [
+  f'{gen}/Makefile',
 
-  generator = env.Program('generator', generator_cpp, LIBS=acado_libs, CPPPATH=cpp_path,
-                          CCFLAGS=env['CCFLAGS'] + ["-Wno-deprecated", "-Wno-overloaded-shift-op-parentheses"])
+  f'{gen}/main_long.c',
+  f'{gen}/acados_solver_long.h',
 
-  cmd = f"cd {Dir('.').get_abspath()} && {generator[0].get_abspath()}"
-  env.Command(generated_c + generated_h, generator, cmd)
+  f'{gen}/long_model/long_expl_vde_adj.c',
 
+  f'{gen}/long_model/long_model.h',
+  f'{gen}/long_constraints/long_h_constraint.h',
+  f'{gen}/long_constraints/long_h_e_constraint.h',
+  f'{gen}/long_cost/long_cost_y_fun.h',
+  f'{gen}/long_cost/long_cost_y_e_fun.h',
+  f'{gen}/long_cost/long_cost_y_0_fun.h',
+] + build_files
 
-mpc_files = ["longitudinal_mpc.c"] + generated_c
-env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path)
+lenv = env.Clone()
+lenv.Clean(generated_files, Dir(gen))
+
+lenv.Command(generated_files,
+             ["long_mpc.py"],
+             f"cd {Dir('.').abspath} && python long_mpc.py")
+
+lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
+lenv["CCFLAGS"].append("-Wno-unused")
+lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
+lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
+                   build_files,
+                   LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp
deleted file mode 100644
index d308d102d..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/generator.cpp
+++ /dev/null
@@ -1,82 +0,0 @@
-#include <acado_code_generation.hpp>
-#include "selfdrive/common/modeldata.h"
-
-using namespace std;
-
-
-int main( )
-{
-  USING_NAMESPACE_ACADO
-
-
-  DifferentialEquation f;
-
-  DifferentialState x_ego, v_ego, a_ego;
-  DifferentialState dummy_0;
-  OnlineData min_a, max_a;
-
-  Control j_ego, accel_slack;
-
-  // Equations of motion
-  f << dot(x_ego) == v_ego;
-  f << dot(v_ego) == a_ego;
-  f << dot(a_ego) == j_ego;
-  f << dot(dummy_0) == accel_slack;
-
-  // Running cost
-  Function h;
-  h << x_ego;
-  h << v_ego;
-  h << a_ego;
-  h << j_ego;
-  h << accel_slack;
-
-  // Weights are defined in mpc.
-  BMatrix Q(5,5); Q.setAll(true);
-
-  // Terminal cost
-  Function hN;
-  hN << x_ego;
-  hN << v_ego;
-  hN << a_ego;
-
-  // Weights are defined in mpc.
-  BMatrix QN(3,3); QN.setAll(true);
-
-  double T_IDXS_ARR[LON_MPC_N + 1];
-  memcpy(T_IDXS_ARR, T_IDXS, (LON_MPC_N + 1) * sizeof(double));
-  Grid times(LON_MPC_N + 1, T_IDXS_ARR);
-  OCP ocp(times);
-  ocp.subjectTo(f);
-
-  ocp.minimizeLSQ(Q, h);
-  ocp.minimizeLSQEndTerm(QN, hN);
-
-  ocp.subjectTo( 0.0 <= v_ego);
-  ocp.subjectTo( 0.0 <= a_ego - min_a + accel_slack);
-  ocp.subjectTo( a_ego - max_a + accel_slack <= 0.0);
-  ocp.setNOD(2);
-
-  OCPexport mpc(ocp);
-  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
-  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
-  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
-  mpc.set( NUM_INTEGRATOR_STEPS, 1000);
-  mpc.set( MAX_NUM_QP_ITERATIONS, 50);
-  mpc.set( CG_USE_VARIABLE_WEIGHTING_MATRIX, YES);
-
-  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
-  mpc.set( QP_SOLVER, QP_QPOASES );
-  mpc.set( HOTSTART_QP, YES );
-  mpc.set( GENERATE_TEST_FILE, NO);
-  mpc.set( GENERATE_MAKE_FILE, NO );
-  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
-  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
-
-  if (mpc.exportCode( "lib_mpc_export" ) != SUCCESSFUL_RETURN)
-    exit( EXIT_FAILURE );
-
-  mpc.printDimensionsQP( );
-
-  return EXIT_SUCCESS;
-}
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c
deleted file mode 100644
index 6f0bb705c..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.c
+++ /dev/null
@@ -1,212 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_auxiliary_functions.h"
-
-#include <stdio.h>
-
-real_t* acado_getVariablesX( )
-{
-	return acadoVariables.x;
-}
-
-real_t* acado_getVariablesU( )
-{
-	return acadoVariables.u;
-}
-
-#if ACADO_NY > 0
-real_t* acado_getVariablesY( )
-{
-	return acadoVariables.y;
-}
-#endif
-
-#if ACADO_NYN > 0
-real_t* acado_getVariablesYN( )
-{
-	return acadoVariables.yN;
-}
-#endif
-
-real_t* acado_getVariablesX0( )
-{
-#if ACADO_INITIAL_VALUE_FIXED
-	return acadoVariables.x0;
-#else
-	return 0;
-#endif
-}
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( )
-{
-	int i, j;
-	printf("\nDifferential variables:\n[\n");
-	for (i = 0; i < ACADO_N + 1; ++i)
-	{
-		for (j = 0; j < ACADO_NX; ++j)
-			printf("\t%e", acadoVariables.x[i * ACADO_NX + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print control variables. */
-void acado_printControlVariables( )
-{
-	int i, j;
-	printf("\nControl variables:\n[\n");
-	for (i = 0; i < ACADO_N; ++i)
-	{
-		for (j = 0; j < ACADO_NU; ++j)
-			printf("\t%e", acadoVariables.u[i * ACADO_NU + j]);
-		printf("\n");
-	}
-	printf("]\n\n");
-}
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( )
-{
-	printf(
-		"\nACADO Toolkit -- A Toolkit for Automatic Control and Dynamic Optimization.\n"
-		"Copyright (C) 2008-2015 by Boris Houska, Hans Joachim Ferreau,\n" 
-		"Milan Vukov and Rien Quirynen, KU Leuven.\n"
-	);
-	
-	printf(
-		"Developed within the Optimization in Engineering Center (OPTEC) under\n"
-		"supervision of Moritz Diehl. All rights reserved.\n\n"
-		"ACADO Toolkit is distributed under the terms of the GNU Lesser\n"
-		"General Public License 3 in the hope that it will be useful,\n"
-		"but WITHOUT ANY WARRANTY; without even the implied warranty of\n"
-		"MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the\n"
-		"GNU Lesser General Public License for more details.\n\n"
-	);
-}
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-void acado_tic( acado_timer* t )
-{
-	QueryPerformanceFrequency(&t->freq);
-	QueryPerformanceCounter(&t->tic);
-}
-
-real_t acado_toc( acado_timer* t )
-{
-	QueryPerformanceCounter(&t->toc);
-	return ((t->toc.QuadPart - t->tic.QuadPart) / (real_t)t->freq.QuadPart);
-}
-
-
-#elif (defined __APPLE__)
-
-void acado_tic( acado_timer* t )
-{
-    /* read current clock cycles */
-    t->tic = mach_absolute_time();
-}
-
-real_t acado_toc( acado_timer* t )
-{
-
-    uint64_t duration; /* elapsed time in clock cycles*/
-    
-    t->toc = mach_absolute_time();
-    duration = t->toc - t->tic;
-    
-    /*conversion from clock cycles to nanoseconds*/
-    mach_timebase_info(&(t->tinfo));
-    duration *= t->tinfo.numer;
-    duration /= t->tinfo.denom;
-
-    return (real_t)duration / 1e9;
-}
-
-#else
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	gettimeofday(&t->tic, 0);
-}
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timeval temp;
-	
-	gettimeofday(&t->toc, 0);
-    
-	if ((t->toc.tv_usec - t->tic.tv_usec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_usec = 1000000 + t->toc.tv_usec - t->tic.tv_usec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_usec = t->toc.tv_usec - t->tic.tv_usec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_usec / 1e6;
-}
-
-#else
-/* ANSI */
-
-/* read current time */
-void acado_tic( acado_timer* t )
-{
-	clock_gettime(CLOCK_MONOTONIC, &t->tic);
-}
-
-
-/* return time passed since last call to tic on this timer */
-real_t acado_toc( acado_timer* t )
-{
-	struct timespec temp;
-    
-	clock_gettime(CLOCK_MONOTONIC, &t->toc);	
-    
-	if ((t->toc.tv_nsec - t->tic.tv_nsec) < 0)
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec - 1;
-		temp.tv_nsec = 1000000000+t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	else
-	{
-		temp.tv_sec = t->toc.tv_sec - t->tic.tv_sec;
-		temp.tv_nsec = t->toc.tv_nsec - t->tic.tv_nsec;
-	}
-	
-	return (real_t)temp.tv_sec + (real_t)temp.tv_nsec / 1e9;
-}
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || _WIN64) */
-
-#endif
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h
deleted file mode 100644
index b1be0a661..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_auxiliary_functions.h
+++ /dev/null
@@ -1,138 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_AUXILIARY_FUNCTIONS_H
-#define ACADO_AUXILIARY_FUNCTIONS_H
-
-#include "acado_common.h"
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** Get pointer to the matrix with differential variables. */
-real_t* acado_getVariablesX( );
-
-/** Get pointer to the matrix with control variables. */
-real_t* acado_getVariablesU( );
-
-#if ACADO_NY > 0
-/** Get pointer to the matrix with references/measurements. */
-real_t* acado_getVariablesY( );
-#endif
-
-#if ACADO_NYN > 0
-/** Get pointer to the vector with references/measurement on the last node. */
-real_t* acado_getVariablesYN( );
-#endif
-
-/** Get pointer to the current state feedback vector. Only applicable for NMPC. */
-real_t* acado_getVariablesX0( );
-
-/** Print differential variables. */
-void acado_printDifferentialVariables( );
-
-/** Print control variables. */
-void acado_printControlVariables( );
-
-/** Print ACADO code generation notice. */
-void acado_printHeader( );
-
-/*
- * A huge thanks goes to Alexander Domahidi from ETHZ, Switzerland, for 
- * providing us with the following timing routines.
- */
-
-#if !(defined _DSPACE)
-#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
-
-/* Use Windows QueryPerformanceCounter for timing. */
-#include <Windows.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	LARGE_INTEGER tic;
-	LARGE_INTEGER toc;
-	LARGE_INTEGER freq;
-} acado_timer;
-
-
-#elif (defined __APPLE__)
-
-#include "unistd.h"
-#include <mach/mach_time.h>
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	uint64_t tic;
-	uint64_t toc;
-	mach_timebase_info_data_t tinfo;
-} acado_timer;
-
-#else
-
-/* Use POSIX clock_gettime() for timing on non-Windows machines. */
-#include <time.h>
-
-#if __STDC_VERSION__ >= 199901L
-/* C99 mode of operation. */
-
-#include <sys/stat.h>
-#include <sys/time.h>
-
-typedef struct acado_timer_
-{
-	struct timeval tic;
-	struct timeval toc;
-} acado_timer;
-
-#else
-/* ANSI C */
-
-/** A structure for keeping internal timer data. */
-typedef struct acado_timer_
-{
-	struct timespec tic;
-	struct timespec toc;
-} acado_timer;
-
-#endif /* __STDC_VERSION__ >= 199901L */
-
-#endif /* (defined _WIN32 || defined _WIN64) */
-
-/** A function for measurement of the current time. */
-void acado_tic( acado_timer* t );
-
-/** A function which returns the elapsed time. */
-real_t acado_toc( acado_timer* t );
-
-#endif
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_AUXILIARY_FUNCTIONS_H */
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_common.h
deleted file mode 100644
index 4951fae46..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_common.h
+++ /dev/null
@@ -1,372 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef ACADO_COMMON_H
-#define ACADO_COMMON_H
-
-#include <math.h>
-#include <string.h>
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-extern "C"
-{
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-/** \defgroup ACADO ACADO CGT generated module. */
-/** @{ */
-
-/** qpOASES QP solver indicator. */
-#define ACADO_QPOASES  0
-#define ACADO_QPOASES3 1
-/** FORCES QP solver indicator.*/
-#define ACADO_FORCES   2
-/** qpDUNES QP solver indicator.*/
-#define ACADO_QPDUNES  3
-/** HPMPC QP solver indicator. */
-#define ACADO_HPMPC    4
-#define ACADO_GENERIC    5
-
-/** Indicator for determining the QP solver used by the ACADO solver code. */
-#define ACADO_QP_SOLVER ACADO_QPOASES
-
-#include "acado_qpoases_interface.hpp"
-
-
-/*
- * Common definitions
- */
-/** User defined block based condensing. */
-#define ACADO_BLOCK_CONDENSING 0
-/** Compute covariance matrix of the last state estimate. */
-#define ACADO_COMPUTE_COVARIANCE_MATRIX 0
-/** Flag indicating whether constraint values are hard-coded or not. */
-#define ACADO_HARDCODED_CONSTRAINT_VALUES 1
-/** Indicator for fixed initial state. */
-#define ACADO_INITIAL_STATE_FIXED 1
-/** Number of control/estimation intervals. */
-#define ACADO_N 32
-/** Number of online data values. */
-#define ACADO_NOD 2
-/** Number of path constraints. */
-#define ACADO_NPAC 2
-/** Number of control variables. */
-#define ACADO_NU 2
-/** Number of differential variables. */
-#define ACADO_NX 4
-/** Number of algebraic variables. */
-#define ACADO_NXA 0
-/** Number of differential derivative variables. */
-#define ACADO_NXD 0
-/** Number of references/measurements per node on the first N nodes. */
-#define ACADO_NY 5
-/** Number of references/measurements on the last (N + 1)st node. */
-#define ACADO_NYN 3
-/** Total number of QP optimization variables. */
-#define ACADO_QP_NV 68
-/** Number of Runge-Kutta stages per integration step. */
-#define ACADO_RK_NSTAGES 4
-/** Providing interface for arrival cost. */
-#define ACADO_USE_ARRIVAL_COST 0
-/** Indicator for usage of non-hard-coded linear terms in the objective. */
-#define ACADO_USE_LINEAR_TERMS 0
-/** Indicator for type of fixed weighting matrices. */
-#define ACADO_WEIGHTING_MATRICES_TYPE 2
-
-
-/*
- * Globally used structure definitions
- */
-
-/** The structure containing the user data.
- * 
- *  Via this structure the user "communicates" with the solver code.
- */
-typedef struct ACADOvariables_
-{
-int dummy;
-/** Matrix of size: 33 x 4 (row major format)
- * 
- *  Matrix containing 33 differential variable vectors.
- */
-real_t x[ 132 ];
-
-/** Matrix of size: 32 x 2 (row major format)
- * 
- *  Matrix containing 32 control variable vectors.
- */
-real_t u[ 64 ];
-
-/** Matrix of size: 33 x 2 (row major format)
- * 
- *  Matrix containing 33 online data vectors.
- */
-real_t od[ 66 ];
-
-/** Column vector of size: 160
- * 
- *  Matrix containing 32 reference/measurement vectors of size 5 for first 32 nodes.
- */
-real_t y[ 160 ];
-
-/** Column vector of size: 3
- * 
- *  Reference/measurement vector for the 33. node.
- */
-real_t yN[ 3 ];
-
-/** Matrix of size: 160 x 5 (row major format) */
-real_t W[ 800 ];
-
-/** Matrix of size: 3 x 3 (row major format) */
-real_t WN[ 9 ];
-
-/** Column vector of size: 4
- * 
- *  Current state feedback vector.
- */
-real_t x0[ 4 ];
-
-
-} ACADOvariables;
-
-/** Private workspace used by the auto-generated code.
- * 
- *  Data members of this structure are private to the solver.
- *  In other words, the user code should not modify values of this 
- *  structure. 
- */
-typedef struct ACADOworkspace_
-{
-real_t rk_ttt;
-
-/** Row vector of size: 32 */
-real_t rk_xxx[ 32 ];
-
-/** Matrix of size: 4 x 28 (row major format) */
-real_t rk_kkk[ 112 ];
-
-/** Row vector of size: 32 */
-real_t state[ 32 ];
-
-/** Column vector of size: 128 */
-real_t d[ 128 ];
-
-/** Column vector of size: 160 */
-real_t Dy[ 160 ];
-
-/** Column vector of size: 3 */
-real_t DyN[ 3 ];
-
-/** Matrix of size: 128 x 4 (row major format) */
-real_t evGx[ 512 ];
-
-/** Matrix of size: 128 x 2 (row major format) */
-real_t evGu[ 256 ];
-
-/** Row vector of size: 8 */
-real_t objValueIn[ 8 ];
-
-/** Row vector of size: 5 */
-real_t objValueOut[ 5 ];
-
-/** Matrix of size: 128 x 4 (row major format) */
-real_t Q1[ 512 ];
-
-/** Matrix of size: 128 x 5 (row major format) */
-real_t Q2[ 640 ];
-
-/** Matrix of size: 64 x 2 (row major format) */
-real_t R1[ 128 ];
-
-/** Matrix of size: 64 x 5 (row major format) */
-real_t R2[ 320 ];
-
-/** Matrix of size: 128 x 2 (row major format) */
-real_t S1[ 256 ];
-
-/** Matrix of size: 4 x 4 (row major format) */
-real_t QN1[ 16 ];
-
-/** Matrix of size: 4 x 3 (row major format) */
-real_t QN2[ 12 ];
-
-/** Column vector of size: 12 */
-real_t conAuxVar[ 12 ];
-
-/** Row vector of size: 8 */
-real_t conValueIn[ 8 ];
-
-/** Row vector of size: 14 */
-real_t conValueOut[ 14 ];
-
-/** Column vector of size: 64 */
-real_t evH[ 64 ];
-
-/** Matrix of size: 64 x 4 (row major format) */
-real_t evHx[ 256 ];
-
-/** Matrix of size: 64 x 2 (row major format) */
-real_t evHu[ 128 ];
-
-/** Column vector of size: 2 */
-real_t evHxd[ 2 ];
-
-/** Column vector of size: 4 */
-real_t Dx0[ 4 ];
-
-/** Matrix of size: 4 x 4 (row major format) */
-real_t T[ 16 ];
-
-/** Matrix of size: 2112 x 2 (row major format) */
-real_t E[ 4224 ];
-
-/** Matrix of size: 2112 x 2 (row major format) */
-real_t QE[ 4224 ];
-
-/** Matrix of size: 128 x 4 (row major format) */
-real_t QGx[ 512 ];
-
-/** Column vector of size: 128 */
-real_t Qd[ 128 ];
-
-/** Column vector of size: 132 */
-real_t QDy[ 132 ];
-
-/** Matrix of size: 64 x 4 (row major format) */
-real_t H10[ 256 ];
-
-/** Matrix of size: 68 x 68 (row major format) */
-real_t H[ 4624 ];
-
-/** Matrix of size: 96 x 68 (row major format) */
-real_t A[ 6528 ];
-
-/** Column vector of size: 68 */
-real_t g[ 68 ];
-
-/** Column vector of size: 68 */
-real_t lb[ 68 ];
-
-/** Column vector of size: 68 */
-real_t ub[ 68 ];
-
-/** Column vector of size: 96 */
-real_t lbA[ 96 ];
-
-/** Column vector of size: 96 */
-real_t ubA[ 96 ];
-
-/** Column vector of size: 68 */
-real_t x[ 68 ];
-
-/** Column vector of size: 164 */
-real_t y[ 164 ];
-
-
-} ACADOworkspace;
-
-/* 
- * Forward function declarations. 
- */
-
-
-/** Performs the integration and sensitivity propagation for one shooting interval.
- *
- *  \param rk_eta Working array to pass the input values and return the results.
- *  \param resetIntegrator The internal memory of the integrator can be reset.
- *  \param rk_index Number of the shooting interval.
- *
- *  \return Status code of the integrator.
- */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index );
-
-/** Export of an ACADO symbolic function.
- *
- *  \param in Input to the exported function.
- *  \param out Output of the exported function.
- */
-void acado_rhs_forw(const real_t* in, real_t* out);
-
-/** Preparation step of the RTI scheme.
- *
- *  \return Status of the integration module. =0: OK, otherwise the error code.
- */
-int acado_preparationStep(  );
-
-/** Feedback/estimation step of the RTI scheme.
- *
- *  \return Status code of the qpOASES QP solver.
- */
-int acado_feedbackStep(  );
-
-/** Solver initialization. Must be called once before any other function call.
- *
- *  \return =0: OK, otherwise an error code of a QP solver.
- */
-int acado_initializeSolver(  );
-
-/** Initialize shooting nodes by a forward simulation starting from the first node.
- */
-void acado_initializeNodesByForwardSimulation(  );
-
-/** Shift differential variables vector by one interval.
- *
- *  \param strategy Shifting strategy: 1. Initialize node 33 with xEnd. 2. Initialize node 33 by forward simulation.
- *  \param xEnd Value for the x vector on the last node. If =0 the old value is used.
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd );
-
-/** Shift controls vector by one interval.
- *
- *  \param uEnd Value for the u vector on the second to last node. If =0 the old value is used.
- */
-void acado_shiftControls( real_t* const uEnd );
-
-/** Get the KKT tolerance of the current iterate.
- *
- *  \return The KKT tolerance value.
- */
-real_t acado_getKKT(  );
-
-/** Calculate the objective value.
- *
- *  \return Value of the objective function.
- */
-real_t acado_getObjective(  );
-
-
-/* 
- * Extern declarations. 
- */
-
-extern ACADOworkspace acadoWorkspace;
-extern ACADOvariables acadoVariables;
-
-/** @} */
-
-#ifndef __MATLAB__
-#ifdef __cplusplus
-} /* extern "C" */
-#endif /* __cplusplus */
-#endif /* __MATLAB__ */
-
-#endif /* ACADO_COMMON_H */
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_integrator.c
deleted file mode 100644
index 627f5398f..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_integrator.c
+++ /dev/null
@@ -1,250 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-void acado_rhs_forw(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 28;
-
-/* Compute outputs: */
-out[0] = xd[1];
-out[1] = xd[2];
-out[2] = u[0];
-out[3] = u[1];
-out[4] = xd[8];
-out[5] = xd[9];
-out[6] = xd[10];
-out[7] = xd[11];
-out[8] = xd[12];
-out[9] = xd[13];
-out[10] = xd[14];
-out[11] = xd[15];
-out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = (real_t)(0.0000000000000000e+00);
-out[14] = (real_t)(0.0000000000000000e+00);
-out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = (real_t)(0.0000000000000000e+00);
-out[18] = (real_t)(0.0000000000000000e+00);
-out[19] = (real_t)(0.0000000000000000e+00);
-out[20] = xd[22];
-out[21] = xd[23];
-out[22] = xd[24];
-out[23] = xd[25];
-out[24] = (real_t)(1.0000000000000000e+00);
-out[25] = (real_t)(0.0000000000000000e+00);
-out[26] = (real_t)(0.0000000000000000e+00);
-out[27] = (real_t)(1.0000000000000000e+00);
-}
-
-/* Fixed step size:0.01 */
-int acado_integrate( real_t* const rk_eta, int resetIntegrator, int rk_index )
-{
-int error;
-
-int run1;
-int numSteps[32] = {1, 3, 5, 7, 9, 11, 13, 15, 17, 19, 21, 22, 24, 26, 28, 30, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62};
-int numInts = numSteps[rk_index];
-acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
-rk_eta[4] = 1.0000000000000000e+00;
-rk_eta[5] = 0.0000000000000000e+00;
-rk_eta[6] = 0.0000000000000000e+00;
-rk_eta[7] = 0.0000000000000000e+00;
-rk_eta[8] = 0.0000000000000000e+00;
-rk_eta[9] = 1.0000000000000000e+00;
-rk_eta[10] = 0.0000000000000000e+00;
-rk_eta[11] = 0.0000000000000000e+00;
-rk_eta[12] = 0.0000000000000000e+00;
-rk_eta[13] = 0.0000000000000000e+00;
-rk_eta[14] = 1.0000000000000000e+00;
-rk_eta[15] = 0.0000000000000000e+00;
-rk_eta[16] = 0.0000000000000000e+00;
-rk_eta[17] = 0.0000000000000000e+00;
-rk_eta[18] = 0.0000000000000000e+00;
-rk_eta[19] = 1.0000000000000000e+00;
-rk_eta[20] = 0.0000000000000000e+00;
-rk_eta[21] = 0.0000000000000000e+00;
-rk_eta[22] = 0.0000000000000000e+00;
-rk_eta[23] = 0.0000000000000000e+00;
-rk_eta[24] = 0.0000000000000000e+00;
-rk_eta[25] = 0.0000000000000000e+00;
-rk_eta[26] = 0.0000000000000000e+00;
-rk_eta[27] = 0.0000000000000000e+00;
-acadoWorkspace.rk_xxx[28] = rk_eta[28];
-acadoWorkspace.rk_xxx[29] = rk_eta[29];
-acadoWorkspace.rk_xxx[30] = rk_eta[30];
-acadoWorkspace.rk_xxx[31] = rk_eta[31];
-
-for (run1 = 0; run1 < 1; ++run1)
-{
-for(run1 = 0; run1 < numInts; run1++ ) {
-acadoWorkspace.rk_xxx[0] = + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + rk_eta[27];
-acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
-acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[0] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[1] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[2] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[3] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[4] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[5] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[6] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[7] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[8] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[9] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[10] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[11] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[12] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[13] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[15] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[16] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[17] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[18] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[19] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[20] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[21] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[22] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[23] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[24] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[25] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[26] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[27] + rk_eta[27];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 28 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[28] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[29] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[30] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[31] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[32] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[33] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[34] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[35] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[36] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[37] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[38] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[39] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[40] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[41] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[42] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[43] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[44] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[45] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[46] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[47] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[48] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[49] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[50] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[51] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[52] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[53] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[54] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)5.0000000000000001e-03*acadoWorkspace.rk_kkk[55] + rk_eta[27];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 56 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[56] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[57] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[58] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[59] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[60] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[61] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[62] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[63] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[64] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[65] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[66] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[67] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[68] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[69] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[70] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[71] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[72] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[73] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[74] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[75] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[76] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[77] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[78] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[79] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[80] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[81] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[82] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)1.0000000000000000e-02*acadoWorkspace.rk_kkk[83] + rk_eta[27];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 84 ]) );
-rk_eta[0] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[0] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[28] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[56] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[84];
-rk_eta[1] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[1] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[29] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[57] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[85];
-rk_eta[2] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[2] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[58] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[86];
-rk_eta[3] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[3] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[59] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[87];
-rk_eta[4] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[4] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[60] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[88];
-rk_eta[5] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[5] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[61] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[89];
-rk_eta[6] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[6] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[62] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[90];
-rk_eta[7] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[7] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[63] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[91];
-rk_eta[8] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[8] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[64] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[92];
-rk_eta[9] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[9] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[65] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[93];
-rk_eta[10] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[10] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[66] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[94];
-rk_eta[11] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[11] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[67] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[95];
-rk_eta[12] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[12] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[68] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[96];
-rk_eta[13] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[13] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[69] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[97];
-rk_eta[14] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[14] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[70] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[98];
-rk_eta[15] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[15] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[71] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[99];
-rk_eta[16] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[16] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[72] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[100];
-rk_eta[17] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[17] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[45] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[73] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[101];
-rk_eta[18] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[18] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[46] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[74] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[102];
-rk_eta[19] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[19] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[47] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[75] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[103];
-rk_eta[20] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[20] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[48] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[76] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[104];
-rk_eta[21] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[21] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[49] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[77] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[105];
-rk_eta[22] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[22] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[50] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[78] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[106];
-rk_eta[23] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[23] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[51] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[79] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[107];
-rk_eta[24] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[24] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[52] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[80] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[108];
-rk_eta[25] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[25] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[53] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[81] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[109];
-rk_eta[26] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[26] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[54] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[82] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[110];
-rk_eta[27] += + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[27] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[55] + (real_t)3.3333333333333331e-03*acadoWorkspace.rk_kkk[83] + (real_t)1.6666666666666666e-03*acadoWorkspace.rk_kkk[111];
-acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
-}
-}
-error = 0;
-return error;
-}
-
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp
deleted file mode 100644
index 1c468288e..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.cpp
+++ /dev/null
@@ -1,70 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-extern "C"
-{
-#include "acado_common.h"
-}
-
-#include "INCLUDE/QProblem.hpp"
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-#include "INCLUDE/EXTRAS/SolutionAnalysis.hpp"
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-static int acado_nWSR;
-
-
-
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-static SolutionAnalysis acado_sa;
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-int acado_solve( void )
-{
-	acado_nWSR = QPOASES_NWSRMAX;
-
-	QProblem qp(68, 96);
-	
-	returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
-
-    qp.getPrimalSolution( acadoWorkspace.x );
-    qp.getDualSolution( acadoWorkspace.y );
-	
-#if ACADO_COMPUTE_COVARIANCE_MATRIX == 1
-
-	if (retVal != SUCCESSFUL_RETURN)
-		return (int)retVal;
-		
-	retVal = acado_sa.getHessianInverse( &qp,var );
-
-#endif /* ACADO_COMPUTE_COVARIANCE_MATRIX */
-
-	return (int)retVal;
-}
-
-int acado_getNWSR( void )
-{
-	return acado_nWSR;
-}
-
-const char* acado_getErrorString( int error )
-{
-	return MessageHandling::getErrorString( error );
-}
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp
deleted file mode 100644
index cad2ee200..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_qpoases_interface.hpp
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#ifndef QPOASES_HEADER
-#define QPOASES_HEADER
-
-#ifdef PC_DEBUG
-#include <stdio.h>
-#endif /* PC_DEBUG */
-
-#include <math.h>
-
-#ifdef __cplusplus
-#define EXTERNC extern "C"
-#else
-#define EXTERNC
-#endif
-
-/*
- * A set of options for qpOASES
- */
-
-/** Maximum number of optimization variables. */
-#define QPOASES_NVMAX      68
-/** Maximum number of constraints. */
-#define QPOASES_NCMAX      96
-/** Maximum number of working set recalculations. */
-#define QPOASES_NWSRMAX    50
-/** Print level for qpOASES. */
-#define QPOASES_PRINTLEVEL PL_NONE
-/** The value of EPS */
-#define QPOASES_EPS        2.221e-16
-/** Internally used floating point type */
-typedef double real_t;
-
-/*
- * Forward function declarations
- */
-
-/** A function that calls the QP solver */
-EXTERNC int acado_solve( void );
-
-/** Get the number of active set changes */
-EXTERNC int acado_getNWSR( void );
-
-/** Get the error string. */
-const char* acado_getErrorString( int error );
-
-#endif /* QPOASES_HEADER */
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_solver.c
deleted file mode 100644
index 4cd0613bf..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/lib_mpc_export/acado_solver.c
+++ /dev/null
@@ -1,1932 +0,0 @@
-/*
- *    This file was auto-generated using the ACADO Toolkit.
- *    
- *    While ACADO Toolkit is free software released under the terms of
- *    the GNU Lesser General Public License (LGPL), the generated code
- *    as such remains the property of the user who used ACADO Toolkit
- *    to generate this code. In particular, user dependent data of the code
- *    do not inherit the GNU LGPL license. On the other hand, parts of the
- *    generated code that are a direct copy of source code from the
- *    ACADO Toolkit or the software tools it is based on, remain, as derived
- *    work, automatically covered by the LGPL license.
- *    
- *    ACADO Toolkit is distributed in the hope that it will be useful,
- *    but WITHOUT ANY WARRANTY; without even the implied warranty of
- *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- *    
- */
-
-
-#include "acado_common.h"
-
-
-
-
-/******************************************************************************/
-/*                                                                            */
-/* ACADO code generation                                                      */
-/*                                                                            */
-/******************************************************************************/
-
-
-int acado_modelSimulation(  )
-{
-int ret;
-
-int lRun1;
-ret = 0;
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 4];
-acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 4 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 4 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 4 + 3];
-
-acadoWorkspace.state[28] = acadoVariables.u[lRun1 * 2];
-acadoWorkspace.state[29] = acadoVariables.u[lRun1 * 2 + 1];
-acadoWorkspace.state[30] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.state[31] = acadoVariables.od[lRun1 * 2 + 1];
-
-ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
-
-acadoWorkspace.d[lRun1 * 4] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 4 + 4];
-acadoWorkspace.d[lRun1 * 4 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 4 + 5];
-acadoWorkspace.d[lRun1 * 4 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 4 + 6];
-acadoWorkspace.d[lRun1 * 4 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 4 + 7];
-
-acadoWorkspace.evGx[lRun1 * 16] = acadoWorkspace.state[4];
-acadoWorkspace.evGx[lRun1 * 16 + 1] = acadoWorkspace.state[5];
-acadoWorkspace.evGx[lRun1 * 16 + 2] = acadoWorkspace.state[6];
-acadoWorkspace.evGx[lRun1 * 16 + 3] = acadoWorkspace.state[7];
-acadoWorkspace.evGx[lRun1 * 16 + 4] = acadoWorkspace.state[8];
-acadoWorkspace.evGx[lRun1 * 16 + 5] = acadoWorkspace.state[9];
-acadoWorkspace.evGx[lRun1 * 16 + 6] = acadoWorkspace.state[10];
-acadoWorkspace.evGx[lRun1 * 16 + 7] = acadoWorkspace.state[11];
-acadoWorkspace.evGx[lRun1 * 16 + 8] = acadoWorkspace.state[12];
-acadoWorkspace.evGx[lRun1 * 16 + 9] = acadoWorkspace.state[13];
-acadoWorkspace.evGx[lRun1 * 16 + 10] = acadoWorkspace.state[14];
-acadoWorkspace.evGx[lRun1 * 16 + 11] = acadoWorkspace.state[15];
-acadoWorkspace.evGx[lRun1 * 16 + 12] = acadoWorkspace.state[16];
-acadoWorkspace.evGx[lRun1 * 16 + 13] = acadoWorkspace.state[17];
-acadoWorkspace.evGx[lRun1 * 16 + 14] = acadoWorkspace.state[18];
-acadoWorkspace.evGx[lRun1 * 16 + 15] = acadoWorkspace.state[19];
-
-acadoWorkspace.evGu[lRun1 * 8] = acadoWorkspace.state[20];
-acadoWorkspace.evGu[lRun1 * 8 + 1] = acadoWorkspace.state[21];
-acadoWorkspace.evGu[lRun1 * 8 + 2] = acadoWorkspace.state[22];
-acadoWorkspace.evGu[lRun1 * 8 + 3] = acadoWorkspace.state[23];
-acadoWorkspace.evGu[lRun1 * 8 + 4] = acadoWorkspace.state[24];
-acadoWorkspace.evGu[lRun1 * 8 + 5] = acadoWorkspace.state[25];
-acadoWorkspace.evGu[lRun1 * 8 + 6] = acadoWorkspace.state[26];
-acadoWorkspace.evGu[lRun1 * 8 + 7] = acadoWorkspace.state[27];
-}
-return ret;
-}
-
-void acado_evaluateLSQ(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 4;
-
-/* Compute outputs: */
-out[0] = xd[0];
-out[1] = xd[1];
-out[2] = xd[2];
-out[3] = u[0];
-out[4] = u[1];
-}
-
-void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-
-/* Compute outputs: */
-out[0] = xd[0];
-out[1] = xd[1];
-out[2] = xd[2];
-}
-
-void acado_setObjQ1Q2( real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 )
-{
-tmpQ2[0] = +tmpObjS[0];
-tmpQ2[1] = +tmpObjS[1];
-tmpQ2[2] = +tmpObjS[2];
-tmpQ2[3] = +tmpObjS[3];
-tmpQ2[4] = +tmpObjS[4];
-tmpQ2[5] = +tmpObjS[5];
-tmpQ2[6] = +tmpObjS[6];
-tmpQ2[7] = +tmpObjS[7];
-tmpQ2[8] = +tmpObjS[8];
-tmpQ2[9] = +tmpObjS[9];
-tmpQ2[10] = +tmpObjS[10];
-tmpQ2[11] = +tmpObjS[11];
-tmpQ2[12] = +tmpObjS[12];
-tmpQ2[13] = +tmpObjS[13];
-tmpQ2[14] = +tmpObjS[14];
-tmpQ2[15] = 0.0;
-;
-tmpQ2[16] = 0.0;
-;
-tmpQ2[17] = 0.0;
-;
-tmpQ2[18] = 0.0;
-;
-tmpQ2[19] = 0.0;
-;
-tmpQ1[0] = + tmpQ2[0];
-tmpQ1[1] = + tmpQ2[1];
-tmpQ1[2] = + tmpQ2[2];
-tmpQ1[3] = 0.0;
-;
-tmpQ1[4] = + tmpQ2[5];
-tmpQ1[5] = + tmpQ2[6];
-tmpQ1[6] = + tmpQ2[7];
-tmpQ1[7] = 0.0;
-;
-tmpQ1[8] = + tmpQ2[10];
-tmpQ1[9] = + tmpQ2[11];
-tmpQ1[10] = + tmpQ2[12];
-tmpQ1[11] = 0.0;
-;
-tmpQ1[12] = + tmpQ2[15];
-tmpQ1[13] = + tmpQ2[16];
-tmpQ1[14] = + tmpQ2[17];
-tmpQ1[15] = 0.0;
-;
-}
-
-void acado_setObjR1R2( real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 )
-{
-tmpR2[0] = +tmpObjS[15];
-tmpR2[1] = +tmpObjS[16];
-tmpR2[2] = +tmpObjS[17];
-tmpR2[3] = +tmpObjS[18];
-tmpR2[4] = +tmpObjS[19];
-tmpR2[5] = +tmpObjS[20];
-tmpR2[6] = +tmpObjS[21];
-tmpR2[7] = +tmpObjS[22];
-tmpR2[8] = +tmpObjS[23];
-tmpR2[9] = +tmpObjS[24];
-tmpR1[0] = + tmpR2[3];
-tmpR1[1] = + tmpR2[4];
-tmpR1[2] = + tmpR2[8];
-tmpR1[3] = + tmpR2[9];
-}
-
-void acado_setObjQN1QN2( real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 )
-{
-tmpQN2[0] = +tmpObjSEndTerm[0];
-tmpQN2[1] = +tmpObjSEndTerm[1];
-tmpQN2[2] = +tmpObjSEndTerm[2];
-tmpQN2[3] = +tmpObjSEndTerm[3];
-tmpQN2[4] = +tmpObjSEndTerm[4];
-tmpQN2[5] = +tmpObjSEndTerm[5];
-tmpQN2[6] = +tmpObjSEndTerm[6];
-tmpQN2[7] = +tmpObjSEndTerm[7];
-tmpQN2[8] = +tmpObjSEndTerm[8];
-tmpQN2[9] = 0.0;
-;
-tmpQN2[10] = 0.0;
-;
-tmpQN2[11] = 0.0;
-;
-tmpQN1[0] = + tmpQN2[0];
-tmpQN1[1] = + tmpQN2[1];
-tmpQN1[2] = + tmpQN2[2];
-tmpQN1[3] = 0.0;
-;
-tmpQN1[4] = + tmpQN2[3];
-tmpQN1[5] = + tmpQN2[4];
-tmpQN1[6] = + tmpQN2[5];
-tmpQN1[7] = 0.0;
-;
-tmpQN1[8] = + tmpQN2[6];
-tmpQN1[9] = + tmpQN2[7];
-tmpQN1[10] = + tmpQN2[8];
-tmpQN1[11] = 0.0;
-;
-tmpQN1[12] = + tmpQN2[9];
-tmpQN1[13] = + tmpQN2[10];
-tmpQN1[14] = + tmpQN2[11];
-tmpQN1[15] = 0.0;
-;
-}
-
-void acado_evaluateObjective(  )
-{
-int runObj;
-for (runObj = 0; runObj < 32; ++runObj)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 4];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 4 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 4 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 4 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.u[runObj * 2];
-acadoWorkspace.objValueIn[5] = acadoVariables.u[runObj * 2 + 1];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[runObj * 2];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[runObj * 5] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.Dy[runObj * 5 + 1] = acadoWorkspace.objValueOut[1];
-acadoWorkspace.Dy[runObj * 5 + 2] = acadoWorkspace.objValueOut[2];
-acadoWorkspace.Dy[runObj * 5 + 3] = acadoWorkspace.objValueOut[3];
-acadoWorkspace.Dy[runObj * 5 + 4] = acadoWorkspace.objValueOut[4];
-
-acado_setObjQ1Q2( &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.Q1[ runObj * 16 ]), &(acadoWorkspace.Q2[ runObj * 20 ]) );
-
-acado_setObjR1R2( &(acadoVariables.W[ runObj * 25 ]), &(acadoWorkspace.R1[ runObj * 4 ]), &(acadoWorkspace.R2[ runObj * 10 ]) );
-
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[128];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[129];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[130];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[131];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[64];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[65];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1];
-acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2];
-
-acado_setObjQN1QN2( acadoVariables.WN, acadoWorkspace.QN1, acadoWorkspace.QN2 );
-
-}
-
-void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew )
-{
-dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3];
-dNew[1] += + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3];
-dNew[2] += + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3];
-dNew[3] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3];
-}
-
-void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 )
-{
-Gx2[0] = Gx1[0];
-Gx2[1] = Gx1[1];
-Gx2[2] = Gx1[2];
-Gx2[3] = Gx1[3];
-Gx2[4] = Gx1[4];
-Gx2[5] = Gx1[5];
-Gx2[6] = Gx1[6];
-Gx2[7] = Gx1[7];
-Gx2[8] = Gx1[8];
-Gx2[9] = Gx1[9];
-Gx2[10] = Gx1[10];
-Gx2[11] = Gx1[11];
-Gx2[12] = Gx1[12];
-Gx2[13] = Gx1[13];
-Gx2[14] = Gx1[14];
-Gx2[15] = Gx1[15];
-}
-
-void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 )
-{
-Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[8] + Gx1[3]*Gx2[12];
-Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[9] + Gx1[3]*Gx2[13];
-Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[10] + Gx1[3]*Gx2[14];
-Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[11] + Gx1[3]*Gx2[15];
-Gx3[4] = + Gx1[4]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[6]*Gx2[8] + Gx1[7]*Gx2[12];
-Gx3[5] = + Gx1[4]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[6]*Gx2[9] + Gx1[7]*Gx2[13];
-Gx3[6] = + Gx1[4]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[6]*Gx2[10] + Gx1[7]*Gx2[14];
-Gx3[7] = + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[6]*Gx2[11] + Gx1[7]*Gx2[15];
-Gx3[8] = + Gx1[8]*Gx2[0] + Gx1[9]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[11]*Gx2[12];
-Gx3[9] = + Gx1[8]*Gx2[1] + Gx1[9]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[11]*Gx2[13];
-Gx3[10] = + Gx1[8]*Gx2[2] + Gx1[9]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[11]*Gx2[14];
-Gx3[11] = + Gx1[8]*Gx2[3] + Gx1[9]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[11]*Gx2[15];
-Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[4] + Gx1[14]*Gx2[8] + Gx1[15]*Gx2[12];
-Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[5] + Gx1[14]*Gx2[9] + Gx1[15]*Gx2[13];
-Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[10] + Gx1[15]*Gx2[14];
-Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[11] + Gx1[15]*Gx2[15];
-}
-
-void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[2] + Gx1[2]*Gu1[4] + Gx1[3]*Gu1[6];
-Gu2[1] = + Gx1[0]*Gu1[1] + Gx1[1]*Gu1[3] + Gx1[2]*Gu1[5] + Gx1[3]*Gu1[7];
-Gu2[2] = + Gx1[4]*Gu1[0] + Gx1[5]*Gu1[2] + Gx1[6]*Gu1[4] + Gx1[7]*Gu1[6];
-Gu2[3] = + Gx1[4]*Gu1[1] + Gx1[5]*Gu1[3] + Gx1[6]*Gu1[5] + Gx1[7]*Gu1[7];
-Gu2[4] = + Gx1[8]*Gu1[0] + Gx1[9]*Gu1[2] + Gx1[10]*Gu1[4] + Gx1[11]*Gu1[6];
-Gu2[5] = + Gx1[8]*Gu1[1] + Gx1[9]*Gu1[3] + Gx1[10]*Gu1[5] + Gx1[11]*Gu1[7];
-Gu2[6] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[2] + Gx1[14]*Gu1[4] + Gx1[15]*Gu1[6];
-Gu2[7] = + Gx1[12]*Gu1[1] + Gx1[13]*Gu1[3] + Gx1[14]*Gu1[5] + Gx1[15]*Gu1[7];
-}
-
-void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
-{
-Gu2[0] = Gu1[0];
-Gu2[1] = Gu1[1];
-Gu2[2] = Gu1[2];
-Gu2[3] = Gu1[3];
-Gu2[4] = Gu1[4];
-Gu2[5] = Gu1[5];
-Gu2[6] = Gu1[6];
-Gu2[7] = Gu1[7];
-}
-
-void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 )
-{
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 4)] += + Gu1[0]*Gu2[0] + Gu1[2]*Gu2[2] + Gu1[4]*Gu2[4] + Gu1[6]*Gu2[6];
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 5)] += + Gu1[0]*Gu2[1] + Gu1[2]*Gu2[3] + Gu1[4]*Gu2[5] + Gu1[6]*Gu2[7];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 4)] += + Gu1[1]*Gu2[0] + Gu1[3]*Gu2[2] + Gu1[5]*Gu2[4] + Gu1[7]*Gu2[6];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 5)] += + Gu1[1]*Gu2[1] + Gu1[3]*Gu2[3] + Gu1[5]*Gu2[5] + Gu1[7]*Gu2[7];
-}
-
-void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 )
-{
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 4)] = R11[0];
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 5)] = R11[1];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 4)] = R11[2];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 5)] = R11[3];
-}
-
-void acado_zeroBlockH11( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 4)] = 0.0000000000000000e+00;
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 5)] = 0.0000000000000000e+00;
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 4)] = 0.0000000000000000e+00;
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 5)] = 0.0000000000000000e+00;
-}
-
-void acado_copyHTH( int iRow, int iCol )
-{
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 4)] = acadoWorkspace.H[(iCol * 136 + 272) + (iRow * 2 + 4)];
-acadoWorkspace.H[(iRow * 136 + 272) + (iCol * 2 + 5)] = acadoWorkspace.H[(iCol * 136 + 340) + (iRow * 2 + 4)];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 4)] = acadoWorkspace.H[(iCol * 136 + 272) + (iRow * 2 + 5)];
-acadoWorkspace.H[(iRow * 136 + 340) + (iCol * 2 + 5)] = acadoWorkspace.H[(iCol * 136 + 340) + (iRow * 2 + 5)];
-}
-
-void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3];
-dNew[1] = + Gx1[4]*dOld[0] + Gx1[5]*dOld[1] + Gx1[6]*dOld[2] + Gx1[7]*dOld[3];
-dNew[2] = + Gx1[8]*dOld[0] + Gx1[9]*dOld[1] + Gx1[10]*dOld[2] + Gx1[11]*dOld[3];
-dNew[3] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3];
-}
-
-void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew )
-{
-dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3];
-dNew[1] = + acadoWorkspace.QN1[4]*dOld[0] + acadoWorkspace.QN1[5]*dOld[1] + acadoWorkspace.QN1[6]*dOld[2] + acadoWorkspace.QN1[7]*dOld[3];
-dNew[2] = + acadoWorkspace.QN1[8]*dOld[0] + acadoWorkspace.QN1[9]*dOld[1] + acadoWorkspace.QN1[10]*dOld[2] + acadoWorkspace.QN1[11]*dOld[3];
-dNew[3] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3];
-}
-
-void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 )
-{
-RDy1[0] = + R2[0]*Dy1[0] + R2[1]*Dy1[1] + R2[2]*Dy1[2] + R2[3]*Dy1[3] + R2[4]*Dy1[4];
-RDy1[1] = + R2[5]*Dy1[0] + R2[6]*Dy1[1] + R2[7]*Dy1[2] + R2[8]*Dy1[3] + R2[9]*Dy1[4];
-}
-
-void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 )
-{
-QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3] + Q2[4]*Dy1[4];
-QDy1[1] = + Q2[5]*Dy1[0] + Q2[6]*Dy1[1] + Q2[7]*Dy1[2] + Q2[8]*Dy1[3] + Q2[9]*Dy1[4];
-QDy1[2] = + Q2[10]*Dy1[0] + Q2[11]*Dy1[1] + Q2[12]*Dy1[2] + Q2[13]*Dy1[3] + Q2[14]*Dy1[4];
-QDy1[3] = + Q2[15]*Dy1[0] + Q2[16]*Dy1[1] + Q2[17]*Dy1[2] + Q2[18]*Dy1[3] + Q2[19]*Dy1[4];
-}
-
-void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 )
-{
-U1[0] += + E1[0]*QDy1[0] + E1[2]*QDy1[1] + E1[4]*QDy1[2] + E1[6]*QDy1[3];
-U1[1] += + E1[1]*QDy1[0] + E1[3]*QDy1[1] + E1[5]*QDy1[2] + E1[7]*QDy1[3];
-}
-
-void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 )
-{
-H101[0] += + E1[0]*Gx1[0] + E1[2]*Gx1[4] + E1[4]*Gx1[8] + E1[6]*Gx1[12];
-H101[1] += + E1[0]*Gx1[1] + E1[2]*Gx1[5] + E1[4]*Gx1[9] + E1[6]*Gx1[13];
-H101[2] += + E1[0]*Gx1[2] + E1[2]*Gx1[6] + E1[4]*Gx1[10] + E1[6]*Gx1[14];
-H101[3] += + E1[0]*Gx1[3] + E1[2]*Gx1[7] + E1[4]*Gx1[11] + E1[6]*Gx1[15];
-H101[4] += + E1[1]*Gx1[0] + E1[3]*Gx1[4] + E1[5]*Gx1[8] + E1[7]*Gx1[12];
-H101[5] += + E1[1]*Gx1[1] + E1[3]*Gx1[5] + E1[5]*Gx1[9] + E1[7]*Gx1[13];
-H101[6] += + E1[1]*Gx1[2] + E1[3]*Gx1[6] + E1[5]*Gx1[10] + E1[7]*Gx1[14];
-H101[7] += + E1[1]*Gx1[3] + E1[3]*Gx1[7] + E1[5]*Gx1[11] + E1[7]*Gx1[15];
-}
-
-void acado_zeroBlockH10( real_t* const H101 )
-{
-{ int lCopy; for (lCopy = 0; lCopy < 8; lCopy++) H101[ lCopy ] = 0; }
-}
-
-void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew )
-{
-dNew[0] += + E1[0]*U1[0] + E1[1]*U1[1];
-dNew[1] += + E1[2]*U1[0] + E1[3]*U1[1];
-dNew[2] += + E1[4]*U1[0] + E1[5]*U1[1];
-dNew[3] += + E1[6]*U1[0] + E1[7]*U1[1];
-}
-
-void acado_zeroBlockH00(  )
-{
-acadoWorkspace.H[0] = 0.0000000000000000e+00;
-acadoWorkspace.H[1] = 0.0000000000000000e+00;
-acadoWorkspace.H[2] = 0.0000000000000000e+00;
-acadoWorkspace.H[3] = 0.0000000000000000e+00;
-acadoWorkspace.H[68] = 0.0000000000000000e+00;
-acadoWorkspace.H[69] = 0.0000000000000000e+00;
-acadoWorkspace.H[70] = 0.0000000000000000e+00;
-acadoWorkspace.H[71] = 0.0000000000000000e+00;
-acadoWorkspace.H[136] = 0.0000000000000000e+00;
-acadoWorkspace.H[137] = 0.0000000000000000e+00;
-acadoWorkspace.H[138] = 0.0000000000000000e+00;
-acadoWorkspace.H[139] = 0.0000000000000000e+00;
-acadoWorkspace.H[204] = 0.0000000000000000e+00;
-acadoWorkspace.H[205] = 0.0000000000000000e+00;
-acadoWorkspace.H[206] = 0.0000000000000000e+00;
-acadoWorkspace.H[207] = 0.0000000000000000e+00;
-}
-
-void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 )
-{
-acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[4]*Gx2[4] + Gx1[8]*Gx2[8] + Gx1[12]*Gx2[12];
-acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[4]*Gx2[5] + Gx1[8]*Gx2[9] + Gx1[12]*Gx2[13];
-acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[4]*Gx2[6] + Gx1[8]*Gx2[10] + Gx1[12]*Gx2[14];
-acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[4]*Gx2[7] + Gx1[8]*Gx2[11] + Gx1[12]*Gx2[15];
-acadoWorkspace.H[68] += + Gx1[1]*Gx2[0] + Gx1[5]*Gx2[4] + Gx1[9]*Gx2[8] + Gx1[13]*Gx2[12];
-acadoWorkspace.H[69] += + Gx1[1]*Gx2[1] + Gx1[5]*Gx2[5] + Gx1[9]*Gx2[9] + Gx1[13]*Gx2[13];
-acadoWorkspace.H[70] += + Gx1[1]*Gx2[2] + Gx1[5]*Gx2[6] + Gx1[9]*Gx2[10] + Gx1[13]*Gx2[14];
-acadoWorkspace.H[71] += + Gx1[1]*Gx2[3] + Gx1[5]*Gx2[7] + Gx1[9]*Gx2[11] + Gx1[13]*Gx2[15];
-acadoWorkspace.H[136] += + Gx1[2]*Gx2[0] + Gx1[6]*Gx2[4] + Gx1[10]*Gx2[8] + Gx1[14]*Gx2[12];
-acadoWorkspace.H[137] += + Gx1[2]*Gx2[1] + Gx1[6]*Gx2[5] + Gx1[10]*Gx2[9] + Gx1[14]*Gx2[13];
-acadoWorkspace.H[138] += + Gx1[2]*Gx2[2] + Gx1[6]*Gx2[6] + Gx1[10]*Gx2[10] + Gx1[14]*Gx2[14];
-acadoWorkspace.H[139] += + Gx1[2]*Gx2[3] + Gx1[6]*Gx2[7] + Gx1[10]*Gx2[11] + Gx1[14]*Gx2[15];
-acadoWorkspace.H[204] += + Gx1[3]*Gx2[0] + Gx1[7]*Gx2[4] + Gx1[11]*Gx2[8] + Gx1[15]*Gx2[12];
-acadoWorkspace.H[205] += + Gx1[3]*Gx2[1] + Gx1[7]*Gx2[5] + Gx1[11]*Gx2[9] + Gx1[15]*Gx2[13];
-acadoWorkspace.H[206] += + Gx1[3]*Gx2[2] + Gx1[7]*Gx2[6] + Gx1[11]*Gx2[10] + Gx1[15]*Gx2[14];
-acadoWorkspace.H[207] += + Gx1[3]*Gx2[3] + Gx1[7]*Gx2[7] + Gx1[11]*Gx2[11] + Gx1[15]*Gx2[15];
-}
-
-void acado_multHxC( real_t* const Hx, real_t* const Gx, real_t* const A01 )
-{
-A01[0] = + Hx[0]*Gx[0] + Hx[1]*Gx[4] + Hx[2]*Gx[8] + Hx[3]*Gx[12];
-A01[1] = + Hx[0]*Gx[1] + Hx[1]*Gx[5] + Hx[2]*Gx[9] + Hx[3]*Gx[13];
-A01[2] = + Hx[0]*Gx[2] + Hx[1]*Gx[6] + Hx[2]*Gx[10] + Hx[3]*Gx[14];
-A01[3] = + Hx[0]*Gx[3] + Hx[1]*Gx[7] + Hx[2]*Gx[11] + Hx[3]*Gx[15];
-A01[68] = + Hx[4]*Gx[0] + Hx[5]*Gx[4] + Hx[6]*Gx[8] + Hx[7]*Gx[12];
-A01[69] = + Hx[4]*Gx[1] + Hx[5]*Gx[5] + Hx[6]*Gx[9] + Hx[7]*Gx[13];
-A01[70] = + Hx[4]*Gx[2] + Hx[5]*Gx[6] + Hx[6]*Gx[10] + Hx[7]*Gx[14];
-A01[71] = + Hx[4]*Gx[3] + Hx[5]*Gx[7] + Hx[6]*Gx[11] + Hx[7]*Gx[15];
-}
-
-void acado_multHxE( real_t* const Hx, real_t* const E, int row, int col )
-{
-acadoWorkspace.A[(row * 136 + 2176) + (col * 2 + 4)] = + Hx[0]*E[0] + Hx[1]*E[2] + Hx[2]*E[4] + Hx[3]*E[6];
-acadoWorkspace.A[(row * 136 + 2176) + (col * 2 + 5)] = + Hx[0]*E[1] + Hx[1]*E[3] + Hx[2]*E[5] + Hx[3]*E[7];
-acadoWorkspace.A[(row * 136 + 2244) + (col * 2 + 4)] = + Hx[4]*E[0] + Hx[5]*E[2] + Hx[6]*E[4] + Hx[7]*E[6];
-acadoWorkspace.A[(row * 136 + 2244) + (col * 2 + 5)] = + Hx[4]*E[1] + Hx[5]*E[3] + Hx[6]*E[5] + Hx[7]*E[7];
-}
-
-void acado_macHxd( real_t* const Hx, real_t* const tmpd, real_t* const lbA, real_t* const ubA )
-{
-acadoWorkspace.evHxd[0] = + Hx[0]*tmpd[0] + Hx[1]*tmpd[1] + Hx[2]*tmpd[2] + Hx[3]*tmpd[3];
-acadoWorkspace.evHxd[1] = + Hx[4]*tmpd[0] + Hx[5]*tmpd[1] + Hx[6]*tmpd[2] + Hx[7]*tmpd[3];
-lbA[0] -= acadoWorkspace.evHxd[0];
-lbA[1] -= acadoWorkspace.evHxd[1];
-ubA[0] -= acadoWorkspace.evHxd[0];
-ubA[1] -= acadoWorkspace.evHxd[1];
-}
-
-void acado_evaluatePathConstraints(const real_t* in, real_t* out)
-{
-const real_t* xd = in;
-const real_t* u = in + 4;
-const real_t* od = in + 6;
-/* Vector of auxiliary variables; number of elements: 12. */
-real_t* a = acadoWorkspace.conAuxVar;
-
-/* Compute intermediate quantities: */
-a[0] = (real_t)(0.0000000000000000e+00);
-a[1] = (real_t)(0.0000000000000000e+00);
-a[2] = (real_t)(1.0000000000000000e+00);
-a[3] = (real_t)(0.0000000000000000e+00);
-a[4] = (real_t)(0.0000000000000000e+00);
-a[5] = (real_t)(0.0000000000000000e+00);
-a[6] = (real_t)(1.0000000000000000e+00);
-a[7] = (real_t)(0.0000000000000000e+00);
-a[8] = (real_t)(0.0000000000000000e+00);
-a[9] = (real_t)(1.0000000000000000e+00);
-a[10] = (real_t)(0.0000000000000000e+00);
-a[11] = (real_t)(1.0000000000000000e+00);
-
-/* Compute outputs: */
-out[0] = ((xd[2]-od[0])+u[1]);
-out[1] = ((xd[2]-od[1])+u[1]);
-out[2] = a[0];
-out[3] = a[1];
-out[4] = a[2];
-out[5] = a[3];
-out[6] = a[4];
-out[7] = a[5];
-out[8] = a[6];
-out[9] = a[7];
-out[10] = a[8];
-out[11] = a[9];
-out[12] = a[10];
-out[13] = a[11];
-}
-
-void acado_macCTSlx( real_t* const C0, real_t* const g0 )
-{
-g0[0] += 0.0;
-;
-g0[1] += 0.0;
-;
-g0[2] += 0.0;
-;
-g0[3] += 0.0;
-;
-}
-
-void acado_macETSlu( real_t* const E0, real_t* const g1 )
-{
-g1[0] += 0.0;
-;
-g1[1] += 0.0;
-;
-}
-
-void acado_condensePrep(  )
-{
-int lRun1;
-int lRun2;
-int lRun3;
-int lRun4;
-int lRun5;
-/** Row vector of size: 32 */
-static const int xBoundIndices[ 32 ] = 
-{ 5, 9, 13, 17, 21, 25, 29, 33, 37, 41, 45, 49, 53, 57, 61, 65, 69, 73, 77, 81, 85, 89, 93, 97, 101, 105, 109, 113, 117, 121, 125, 129 };
-acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E );
-for (lRun1 = 1; lRun1 < 32; ++lRun1)
-{
-acado_moveGxT( &(acadoWorkspace.evGx[ lRun1 * 16 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ lRun1 * 4-4 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]), &(acadoWorkspace.d[ lRun1 * 4 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ lRun1 * 16-16 ]), &(acadoWorkspace.evGx[ lRun1 * 16 ]) );
-for (lRun2 = 0; lRun2 < lRun1; ++lRun2)
-{
-lRun4 = (((lRun1) * (lRun1-1)) / (2)) + (lRun2);
-lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2);
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ lRun4 * 8 ]), &(acadoWorkspace.E[ lRun3 * 8 ]) );
-}
-lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2);
-acado_moveGuE( &(acadoWorkspace.evGu[ lRun1 * 8 ]), &(acadoWorkspace.E[ lRun3 * 8 ]) );
-}
-
-acado_multGxGx( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multGxGx( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 320 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) );
-acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) );
-
-for (lRun1 = 0; lRun1 < 31; ++lRun1)
-{
-for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2)
-{
-lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2);
-acado_multGxGu( &(acadoWorkspace.Q1[ lRun1 * 16 + 16 ]), &(acadoWorkspace.E[ lRun3 * 8 ]), &(acadoWorkspace.QE[ lRun3 * 8 ]) );
-}
-}
-
-for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2)
-{
-lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2);
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ lRun3 * 8 ]), &(acadoWorkspace.QE[ lRun3 * 8 ]) );
-}
-
-acado_zeroBlockH00(  );
-acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multCTQC( &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.QGx[ 16 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.QGx[ 32 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.QGx[ 48 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.QGx[ 64 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.QGx[ 80 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.QGx[ 96 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.QGx[ 112 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.QGx[ 128 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.QGx[ 160 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.QGx[ 176 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.QGx[ 192 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.QGx[ 208 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.QGx[ 224 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.QGx[ 240 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.QGx[ 256 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.QGx[ 272 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.QGx[ 304 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.QGx[ 320 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.QGx[ 336 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.QGx[ 352 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.QGx[ 368 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.QGx[ 384 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.QGx[ 400 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.QGx[ 416 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.QGx[ 448 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.QGx[ 464 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.QGx[ 480 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 496 ]), &(acadoWorkspace.QGx[ 496 ]) );
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-acado_zeroBlockH10( &(acadoWorkspace.H10[ lRun1 * 8 ]) );
-for (lRun2 = lRun1; lRun2 < 32; ++lRun2)
-{
-lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1);
-acado_multQETGx( &(acadoWorkspace.QE[ lRun3 * 8 ]), &(acadoWorkspace.evGx[ lRun2 * 16 ]), &(acadoWorkspace.H10[ lRun1 * 8 ]) );
-}
-}
-
-for (lRun2 = 0;lRun2 < 4; ++lRun2)
-for (lRun3 = 0;lRun3 < 64; ++lRun3)
-acadoWorkspace.H[(lRun2 * 68) + (lRun3 + 4)] = acadoWorkspace.H10[(lRun3 * 4) + (lRun2)];
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-acado_setBlockH11_R1( lRun1, lRun1, &(acadoWorkspace.R1[ lRun1 * 4 ]) );
-lRun2 = lRun1;
-for (lRun3 = lRun1; lRun3 < 32; ++lRun3)
-{
-lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1);
-lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2);
-acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 8 ]), &(acadoWorkspace.QE[ lRun5 * 8 ]) );
-}
-for (lRun2 = lRun1 + 1; lRun2 < 32; ++lRun2)
-{
-acado_zeroBlockH11( lRun1, lRun2 );
-for (lRun3 = lRun2; lRun3 < 32; ++lRun3)
-{
-lRun4 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun1);
-lRun5 = (((lRun3 + 1) * (lRun3)) / (2)) + (lRun2);
-acado_setBlockH11( lRun1, lRun2, &(acadoWorkspace.E[ lRun4 * 8 ]), &(acadoWorkspace.QE[ lRun5 * 8 ]) );
-}
-}
-}
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-for (lRun2 = 0; lRun2 < lRun1; ++lRun2)
-{
-acado_copyHTH( lRun1, lRun2 );
-}
-}
-
-for (lRun2 = 0;lRun2 < 64; ++lRun2)
-for (lRun3 = 0;lRun3 < 4; ++lRun3)
-acadoWorkspace.H[(lRun2 * 68 + 272) + (lRun3)] = acadoWorkspace.H10[(lRun2 * 4) + (lRun3)];
-
-acado_multQ1d( &(acadoWorkspace.Q1[ 16 ]), acadoWorkspace.d, acadoWorkspace.Qd );
-acado_multQ1d( &(acadoWorkspace.Q1[ 32 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.Qd[ 4 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 48 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.Qd[ 8 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 64 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 80 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.Qd[ 16 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 96 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.Qd[ 20 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 112 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 128 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.Qd[ 28 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.Qd[ 32 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 160 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 176 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.Qd[ 40 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 192 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.Qd[ 44 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 208 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 224 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.Qd[ 52 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 240 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.Qd[ 56 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 256 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 272 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.Qd[ 64 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.Qd[ 68 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 304 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 320 ]), &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.Qd[ 76 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 336 ]), &(acadoWorkspace.d[ 80 ]), &(acadoWorkspace.Qd[ 80 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 352 ]), &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.Qd[ 84 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 368 ]), &(acadoWorkspace.d[ 88 ]), &(acadoWorkspace.Qd[ 88 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 384 ]), &(acadoWorkspace.d[ 92 ]), &(acadoWorkspace.Qd[ 92 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 400 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.Qd[ 96 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 416 ]), &(acadoWorkspace.d[ 100 ]), &(acadoWorkspace.Qd[ 100 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.d[ 104 ]), &(acadoWorkspace.Qd[ 104 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 448 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.Qd[ 108 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 464 ]), &(acadoWorkspace.d[ 112 ]), &(acadoWorkspace.Qd[ 112 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 480 ]), &(acadoWorkspace.d[ 116 ]), &(acadoWorkspace.Qd[ 116 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 496 ]), &(acadoWorkspace.d[ 120 ]), &(acadoWorkspace.Qd[ 120 ]) );
-acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 124 ]), &(acadoWorkspace.Qd[ 124 ]) );
-
-acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 16 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 32 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 48 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 64 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 80 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 96 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 112 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 128 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 160 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 176 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 192 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 208 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 224 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 240 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 256 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 272 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 304 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 320 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 336 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 352 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 368 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 384 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 400 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 416 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 448 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 464 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 480 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 496 ]), acadoWorkspace.g );
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-for (lRun2 = lRun1; lRun2 < 32; ++lRun2)
-{
-lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1);
-acado_macETSlu( &(acadoWorkspace.QE[ lRun3 * 8 ]), &(acadoWorkspace.g[ lRun1 * 2 + 4 ]) );
-}
-}
-acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
-acadoWorkspace.lb[24] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[20];
-acadoWorkspace.lb[25] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[21];
-acadoWorkspace.lb[26] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[22];
-acadoWorkspace.lb[27] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[23];
-acadoWorkspace.lb[28] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[24];
-acadoWorkspace.lb[29] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[25];
-acadoWorkspace.lb[30] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[26];
-acadoWorkspace.lb[31] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[27];
-acadoWorkspace.lb[32] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[28];
-acadoWorkspace.lb[33] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[29];
-acadoWorkspace.lb[34] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[30];
-acadoWorkspace.lb[35] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[31];
-acadoWorkspace.lb[36] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[32];
-acadoWorkspace.lb[37] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[33];
-acadoWorkspace.lb[38] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[34];
-acadoWorkspace.lb[39] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[35];
-acadoWorkspace.lb[40] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[36];
-acadoWorkspace.lb[41] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[37];
-acadoWorkspace.lb[42] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[38];
-acadoWorkspace.lb[43] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[39];
-acadoWorkspace.lb[44] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[40];
-acadoWorkspace.lb[45] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[41];
-acadoWorkspace.lb[46] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[42];
-acadoWorkspace.lb[47] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[43];
-acadoWorkspace.lb[48] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[44];
-acadoWorkspace.lb[49] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[45];
-acadoWorkspace.lb[50] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[46];
-acadoWorkspace.lb[51] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[47];
-acadoWorkspace.lb[52] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[48];
-acadoWorkspace.lb[53] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[49];
-acadoWorkspace.lb[54] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[50];
-acadoWorkspace.lb[55] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[51];
-acadoWorkspace.lb[56] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[52];
-acadoWorkspace.lb[57] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[53];
-acadoWorkspace.lb[58] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[54];
-acadoWorkspace.lb[59] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[55];
-acadoWorkspace.lb[60] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[56];
-acadoWorkspace.lb[61] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[57];
-acadoWorkspace.lb[62] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[58];
-acadoWorkspace.lb[63] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[59];
-acadoWorkspace.lb[64] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[60];
-acadoWorkspace.lb[65] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[61];
-acadoWorkspace.lb[66] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[62];
-acadoWorkspace.lb[67] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[63];
-acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
-acadoWorkspace.ub[24] = (real_t)1.0000000000000000e+12 - acadoVariables.u[20];
-acadoWorkspace.ub[25] = (real_t)1.0000000000000000e+12 - acadoVariables.u[21];
-acadoWorkspace.ub[26] = (real_t)1.0000000000000000e+12 - acadoVariables.u[22];
-acadoWorkspace.ub[27] = (real_t)1.0000000000000000e+12 - acadoVariables.u[23];
-acadoWorkspace.ub[28] = (real_t)1.0000000000000000e+12 - acadoVariables.u[24];
-acadoWorkspace.ub[29] = (real_t)1.0000000000000000e+12 - acadoVariables.u[25];
-acadoWorkspace.ub[30] = (real_t)1.0000000000000000e+12 - acadoVariables.u[26];
-acadoWorkspace.ub[31] = (real_t)1.0000000000000000e+12 - acadoVariables.u[27];
-acadoWorkspace.ub[32] = (real_t)1.0000000000000000e+12 - acadoVariables.u[28];
-acadoWorkspace.ub[33] = (real_t)1.0000000000000000e+12 - acadoVariables.u[29];
-acadoWorkspace.ub[34] = (real_t)1.0000000000000000e+12 - acadoVariables.u[30];
-acadoWorkspace.ub[35] = (real_t)1.0000000000000000e+12 - acadoVariables.u[31];
-acadoWorkspace.ub[36] = (real_t)1.0000000000000000e+12 - acadoVariables.u[32];
-acadoWorkspace.ub[37] = (real_t)1.0000000000000000e+12 - acadoVariables.u[33];
-acadoWorkspace.ub[38] = (real_t)1.0000000000000000e+12 - acadoVariables.u[34];
-acadoWorkspace.ub[39] = (real_t)1.0000000000000000e+12 - acadoVariables.u[35];
-acadoWorkspace.ub[40] = (real_t)1.0000000000000000e+12 - acadoVariables.u[36];
-acadoWorkspace.ub[41] = (real_t)1.0000000000000000e+12 - acadoVariables.u[37];
-acadoWorkspace.ub[42] = (real_t)1.0000000000000000e+12 - acadoVariables.u[38];
-acadoWorkspace.ub[43] = (real_t)1.0000000000000000e+12 - acadoVariables.u[39];
-acadoWorkspace.ub[44] = (real_t)1.0000000000000000e+12 - acadoVariables.u[40];
-acadoWorkspace.ub[45] = (real_t)1.0000000000000000e+12 - acadoVariables.u[41];
-acadoWorkspace.ub[46] = (real_t)1.0000000000000000e+12 - acadoVariables.u[42];
-acadoWorkspace.ub[47] = (real_t)1.0000000000000000e+12 - acadoVariables.u[43];
-acadoWorkspace.ub[48] = (real_t)1.0000000000000000e+12 - acadoVariables.u[44];
-acadoWorkspace.ub[49] = (real_t)1.0000000000000000e+12 - acadoVariables.u[45];
-acadoWorkspace.ub[50] = (real_t)1.0000000000000000e+12 - acadoVariables.u[46];
-acadoWorkspace.ub[51] = (real_t)1.0000000000000000e+12 - acadoVariables.u[47];
-acadoWorkspace.ub[52] = (real_t)1.0000000000000000e+12 - acadoVariables.u[48];
-acadoWorkspace.ub[53] = (real_t)1.0000000000000000e+12 - acadoVariables.u[49];
-acadoWorkspace.ub[54] = (real_t)1.0000000000000000e+12 - acadoVariables.u[50];
-acadoWorkspace.ub[55] = (real_t)1.0000000000000000e+12 - acadoVariables.u[51];
-acadoWorkspace.ub[56] = (real_t)1.0000000000000000e+12 - acadoVariables.u[52];
-acadoWorkspace.ub[57] = (real_t)1.0000000000000000e+12 - acadoVariables.u[53];
-acadoWorkspace.ub[58] = (real_t)1.0000000000000000e+12 - acadoVariables.u[54];
-acadoWorkspace.ub[59] = (real_t)1.0000000000000000e+12 - acadoVariables.u[55];
-acadoWorkspace.ub[60] = (real_t)1.0000000000000000e+12 - acadoVariables.u[56];
-acadoWorkspace.ub[61] = (real_t)1.0000000000000000e+12 - acadoVariables.u[57];
-acadoWorkspace.ub[62] = (real_t)1.0000000000000000e+12 - acadoVariables.u[58];
-acadoWorkspace.ub[63] = (real_t)1.0000000000000000e+12 - acadoVariables.u[59];
-acadoWorkspace.ub[64] = (real_t)1.0000000000000000e+12 - acadoVariables.u[60];
-acadoWorkspace.ub[65] = (real_t)1.0000000000000000e+12 - acadoVariables.u[61];
-acadoWorkspace.ub[66] = (real_t)1.0000000000000000e+12 - acadoVariables.u[62];
-acadoWorkspace.ub[67] = (real_t)1.0000000000000000e+12 - acadoVariables.u[63];
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-lRun3 = xBoundIndices[ lRun1 ] - 4;
-lRun4 = ((lRun3) / (4)) + (1);
-acadoWorkspace.A[lRun1 * 68] = acadoWorkspace.evGx[lRun3 * 4];
-acadoWorkspace.A[lRun1 * 68 + 1] = acadoWorkspace.evGx[lRun3 * 4 + 1];
-acadoWorkspace.A[lRun1 * 68 + 2] = acadoWorkspace.evGx[lRun3 * 4 + 2];
-acadoWorkspace.A[lRun1 * 68 + 3] = acadoWorkspace.evGx[lRun3 * 4 + 3];
-for (lRun2 = 0; lRun2 < lRun4; ++lRun2)
-{
-lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (4)) + ((lRun3) % (4));
-acadoWorkspace.A[(lRun1 * 68) + (lRun2 * 2 + 4)] = acadoWorkspace.E[lRun5 * 2];
-acadoWorkspace.A[(lRun1 * 68) + (lRun2 * 2 + 5)] = acadoWorkspace.E[lRun5 * 2 + 1];
-}
-}
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-acadoWorkspace.conValueIn[0] = acadoVariables.x[lRun1 * 4];
-acadoWorkspace.conValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
-acadoWorkspace.conValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
-acadoWorkspace.conValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
-acadoWorkspace.conValueIn[4] = acadoVariables.u[lRun1 * 2];
-acadoWorkspace.conValueIn[5] = acadoVariables.u[lRun1 * 2 + 1];
-acadoWorkspace.conValueIn[6] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.conValueIn[7] = acadoVariables.od[lRun1 * 2 + 1];
-acado_evaluatePathConstraints( acadoWorkspace.conValueIn, acadoWorkspace.conValueOut );
-acadoWorkspace.evH[lRun1 * 2] = acadoWorkspace.conValueOut[0];
-acadoWorkspace.evH[lRun1 * 2 + 1] = acadoWorkspace.conValueOut[1];
-
-acadoWorkspace.evHx[lRun1 * 8] = acadoWorkspace.conValueOut[2];
-acadoWorkspace.evHx[lRun1 * 8 + 1] = acadoWorkspace.conValueOut[3];
-acadoWorkspace.evHx[lRun1 * 8 + 2] = acadoWorkspace.conValueOut[4];
-acadoWorkspace.evHx[lRun1 * 8 + 3] = acadoWorkspace.conValueOut[5];
-acadoWorkspace.evHx[lRun1 * 8 + 4] = acadoWorkspace.conValueOut[6];
-acadoWorkspace.evHx[lRun1 * 8 + 5] = acadoWorkspace.conValueOut[7];
-acadoWorkspace.evHx[lRun1 * 8 + 6] = acadoWorkspace.conValueOut[8];
-acadoWorkspace.evHx[lRun1 * 8 + 7] = acadoWorkspace.conValueOut[9];
-acadoWorkspace.evHu[lRun1 * 4] = acadoWorkspace.conValueOut[10];
-acadoWorkspace.evHu[lRun1 * 4 + 1] = acadoWorkspace.conValueOut[11];
-acadoWorkspace.evHu[lRun1 * 4 + 2] = acadoWorkspace.conValueOut[12];
-acadoWorkspace.evHu[lRun1 * 4 + 3] = acadoWorkspace.conValueOut[13];
-}
-
-acadoWorkspace.A[2176] = acadoWorkspace.evHx[0];
-acadoWorkspace.A[2177] = acadoWorkspace.evHx[1];
-acadoWorkspace.A[2178] = acadoWorkspace.evHx[2];
-acadoWorkspace.A[2179] = acadoWorkspace.evHx[3];
-acadoWorkspace.A[2244] = acadoWorkspace.evHx[4];
-acadoWorkspace.A[2245] = acadoWorkspace.evHx[5];
-acadoWorkspace.A[2246] = acadoWorkspace.evHx[6];
-acadoWorkspace.A[2247] = acadoWorkspace.evHx[7];
-
-acado_multHxC( &(acadoWorkspace.evHx[ 8 ]), acadoWorkspace.evGx, &(acadoWorkspace.A[ 2312 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 16 ]), &(acadoWorkspace.evGx[ 16 ]), &(acadoWorkspace.A[ 2448 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 24 ]), &(acadoWorkspace.evGx[ 32 ]), &(acadoWorkspace.A[ 2584 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 32 ]), &(acadoWorkspace.evGx[ 48 ]), &(acadoWorkspace.A[ 2720 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 40 ]), &(acadoWorkspace.evGx[ 64 ]), &(acadoWorkspace.A[ 2856 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 48 ]), &(acadoWorkspace.evGx[ 80 ]), &(acadoWorkspace.A[ 2992 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 56 ]), &(acadoWorkspace.evGx[ 96 ]), &(acadoWorkspace.A[ 3128 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 64 ]), &(acadoWorkspace.evGx[ 112 ]), &(acadoWorkspace.A[ 3264 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 72 ]), &(acadoWorkspace.evGx[ 128 ]), &(acadoWorkspace.A[ 3400 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 80 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.A[ 3536 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 88 ]), &(acadoWorkspace.evGx[ 160 ]), &(acadoWorkspace.A[ 3672 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 96 ]), &(acadoWorkspace.evGx[ 176 ]), &(acadoWorkspace.A[ 3808 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 104 ]), &(acadoWorkspace.evGx[ 192 ]), &(acadoWorkspace.A[ 3944 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 112 ]), &(acadoWorkspace.evGx[ 208 ]), &(acadoWorkspace.A[ 4080 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 120 ]), &(acadoWorkspace.evGx[ 224 ]), &(acadoWorkspace.A[ 4216 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 128 ]), &(acadoWorkspace.evGx[ 240 ]), &(acadoWorkspace.A[ 4352 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 136 ]), &(acadoWorkspace.evGx[ 256 ]), &(acadoWorkspace.A[ 4488 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 144 ]), &(acadoWorkspace.evGx[ 272 ]), &(acadoWorkspace.A[ 4624 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 152 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.A[ 4760 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 160 ]), &(acadoWorkspace.evGx[ 304 ]), &(acadoWorkspace.A[ 4896 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 168 ]), &(acadoWorkspace.evGx[ 320 ]), &(acadoWorkspace.A[ 5032 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 176 ]), &(acadoWorkspace.evGx[ 336 ]), &(acadoWorkspace.A[ 5168 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 184 ]), &(acadoWorkspace.evGx[ 352 ]), &(acadoWorkspace.A[ 5304 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 192 ]), &(acadoWorkspace.evGx[ 368 ]), &(acadoWorkspace.A[ 5440 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 200 ]), &(acadoWorkspace.evGx[ 384 ]), &(acadoWorkspace.A[ 5576 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 208 ]), &(acadoWorkspace.evGx[ 400 ]), &(acadoWorkspace.A[ 5712 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 216 ]), &(acadoWorkspace.evGx[ 416 ]), &(acadoWorkspace.A[ 5848 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 224 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.A[ 5984 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 232 ]), &(acadoWorkspace.evGx[ 448 ]), &(acadoWorkspace.A[ 6120 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 240 ]), &(acadoWorkspace.evGx[ 464 ]), &(acadoWorkspace.A[ 6256 ]) );
-acado_multHxC( &(acadoWorkspace.evHx[ 248 ]), &(acadoWorkspace.evGx[ 480 ]), &(acadoWorkspace.A[ 6392 ]) );
-
-for (lRun2 = 0; lRun2 < 31; ++lRun2)
-{
-for (lRun3 = 0; lRun3 < lRun2 + 1; ++lRun3)
-{
-lRun4 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun3);
-lRun5 = lRun2 + 1;
-acado_multHxE( &(acadoWorkspace.evHx[ lRun2 * 8 + 8 ]), &(acadoWorkspace.E[ lRun4 * 8 ]), lRun5, lRun3 );
-}
-}
-
-acadoWorkspace.A[2180] = acadoWorkspace.evHu[0];
-acadoWorkspace.A[2181] = acadoWorkspace.evHu[1];
-acadoWorkspace.A[2248] = acadoWorkspace.evHu[2];
-acadoWorkspace.A[2249] = acadoWorkspace.evHu[3];
-acadoWorkspace.A[2318] = acadoWorkspace.evHu[4];
-acadoWorkspace.A[2319] = acadoWorkspace.evHu[5];
-acadoWorkspace.A[2386] = acadoWorkspace.evHu[6];
-acadoWorkspace.A[2387] = acadoWorkspace.evHu[7];
-acadoWorkspace.A[2456] = acadoWorkspace.evHu[8];
-acadoWorkspace.A[2457] = acadoWorkspace.evHu[9];
-acadoWorkspace.A[2524] = acadoWorkspace.evHu[10];
-acadoWorkspace.A[2525] = acadoWorkspace.evHu[11];
-acadoWorkspace.A[2594] = acadoWorkspace.evHu[12];
-acadoWorkspace.A[2595] = acadoWorkspace.evHu[13];
-acadoWorkspace.A[2662] = acadoWorkspace.evHu[14];
-acadoWorkspace.A[2663] = acadoWorkspace.evHu[15];
-acadoWorkspace.A[2732] = acadoWorkspace.evHu[16];
-acadoWorkspace.A[2733] = acadoWorkspace.evHu[17];
-acadoWorkspace.A[2800] = acadoWorkspace.evHu[18];
-acadoWorkspace.A[2801] = acadoWorkspace.evHu[19];
-acadoWorkspace.A[2870] = acadoWorkspace.evHu[20];
-acadoWorkspace.A[2871] = acadoWorkspace.evHu[21];
-acadoWorkspace.A[2938] = acadoWorkspace.evHu[22];
-acadoWorkspace.A[2939] = acadoWorkspace.evHu[23];
-acadoWorkspace.A[3008] = acadoWorkspace.evHu[24];
-acadoWorkspace.A[3009] = acadoWorkspace.evHu[25];
-acadoWorkspace.A[3076] = acadoWorkspace.evHu[26];
-acadoWorkspace.A[3077] = acadoWorkspace.evHu[27];
-acadoWorkspace.A[3146] = acadoWorkspace.evHu[28];
-acadoWorkspace.A[3147] = acadoWorkspace.evHu[29];
-acadoWorkspace.A[3214] = acadoWorkspace.evHu[30];
-acadoWorkspace.A[3215] = acadoWorkspace.evHu[31];
-acadoWorkspace.A[3284] = acadoWorkspace.evHu[32];
-acadoWorkspace.A[3285] = acadoWorkspace.evHu[33];
-acadoWorkspace.A[3352] = acadoWorkspace.evHu[34];
-acadoWorkspace.A[3353] = acadoWorkspace.evHu[35];
-acadoWorkspace.A[3422] = acadoWorkspace.evHu[36];
-acadoWorkspace.A[3423] = acadoWorkspace.evHu[37];
-acadoWorkspace.A[3490] = acadoWorkspace.evHu[38];
-acadoWorkspace.A[3491] = acadoWorkspace.evHu[39];
-acadoWorkspace.A[3560] = acadoWorkspace.evHu[40];
-acadoWorkspace.A[3561] = acadoWorkspace.evHu[41];
-acadoWorkspace.A[3628] = acadoWorkspace.evHu[42];
-acadoWorkspace.A[3629] = acadoWorkspace.evHu[43];
-acadoWorkspace.A[3698] = acadoWorkspace.evHu[44];
-acadoWorkspace.A[3699] = acadoWorkspace.evHu[45];
-acadoWorkspace.A[3766] = acadoWorkspace.evHu[46];
-acadoWorkspace.A[3767] = acadoWorkspace.evHu[47];
-acadoWorkspace.A[3836] = acadoWorkspace.evHu[48];
-acadoWorkspace.A[3837] = acadoWorkspace.evHu[49];
-acadoWorkspace.A[3904] = acadoWorkspace.evHu[50];
-acadoWorkspace.A[3905] = acadoWorkspace.evHu[51];
-acadoWorkspace.A[3974] = acadoWorkspace.evHu[52];
-acadoWorkspace.A[3975] = acadoWorkspace.evHu[53];
-acadoWorkspace.A[4042] = acadoWorkspace.evHu[54];
-acadoWorkspace.A[4043] = acadoWorkspace.evHu[55];
-acadoWorkspace.A[4112] = acadoWorkspace.evHu[56];
-acadoWorkspace.A[4113] = acadoWorkspace.evHu[57];
-acadoWorkspace.A[4180] = acadoWorkspace.evHu[58];
-acadoWorkspace.A[4181] = acadoWorkspace.evHu[59];
-acadoWorkspace.A[4250] = acadoWorkspace.evHu[60];
-acadoWorkspace.A[4251] = acadoWorkspace.evHu[61];
-acadoWorkspace.A[4318] = acadoWorkspace.evHu[62];
-acadoWorkspace.A[4319] = acadoWorkspace.evHu[63];
-acadoWorkspace.A[4388] = acadoWorkspace.evHu[64];
-acadoWorkspace.A[4389] = acadoWorkspace.evHu[65];
-acadoWorkspace.A[4456] = acadoWorkspace.evHu[66];
-acadoWorkspace.A[4457] = acadoWorkspace.evHu[67];
-acadoWorkspace.A[4526] = acadoWorkspace.evHu[68];
-acadoWorkspace.A[4527] = acadoWorkspace.evHu[69];
-acadoWorkspace.A[4594] = acadoWorkspace.evHu[70];
-acadoWorkspace.A[4595] = acadoWorkspace.evHu[71];
-acadoWorkspace.A[4664] = acadoWorkspace.evHu[72];
-acadoWorkspace.A[4665] = acadoWorkspace.evHu[73];
-acadoWorkspace.A[4732] = acadoWorkspace.evHu[74];
-acadoWorkspace.A[4733] = acadoWorkspace.evHu[75];
-acadoWorkspace.A[4802] = acadoWorkspace.evHu[76];
-acadoWorkspace.A[4803] = acadoWorkspace.evHu[77];
-acadoWorkspace.A[4870] = acadoWorkspace.evHu[78];
-acadoWorkspace.A[4871] = acadoWorkspace.evHu[79];
-acadoWorkspace.A[4940] = acadoWorkspace.evHu[80];
-acadoWorkspace.A[4941] = acadoWorkspace.evHu[81];
-acadoWorkspace.A[5008] = acadoWorkspace.evHu[82];
-acadoWorkspace.A[5009] = acadoWorkspace.evHu[83];
-acadoWorkspace.A[5078] = acadoWorkspace.evHu[84];
-acadoWorkspace.A[5079] = acadoWorkspace.evHu[85];
-acadoWorkspace.A[5146] = acadoWorkspace.evHu[86];
-acadoWorkspace.A[5147] = acadoWorkspace.evHu[87];
-acadoWorkspace.A[5216] = acadoWorkspace.evHu[88];
-acadoWorkspace.A[5217] = acadoWorkspace.evHu[89];
-acadoWorkspace.A[5284] = acadoWorkspace.evHu[90];
-acadoWorkspace.A[5285] = acadoWorkspace.evHu[91];
-acadoWorkspace.A[5354] = acadoWorkspace.evHu[92];
-acadoWorkspace.A[5355] = acadoWorkspace.evHu[93];
-acadoWorkspace.A[5422] = acadoWorkspace.evHu[94];
-acadoWorkspace.A[5423] = acadoWorkspace.evHu[95];
-acadoWorkspace.A[5492] = acadoWorkspace.evHu[96];
-acadoWorkspace.A[5493] = acadoWorkspace.evHu[97];
-acadoWorkspace.A[5560] = acadoWorkspace.evHu[98];
-acadoWorkspace.A[5561] = acadoWorkspace.evHu[99];
-acadoWorkspace.A[5630] = acadoWorkspace.evHu[100];
-acadoWorkspace.A[5631] = acadoWorkspace.evHu[101];
-acadoWorkspace.A[5698] = acadoWorkspace.evHu[102];
-acadoWorkspace.A[5699] = acadoWorkspace.evHu[103];
-acadoWorkspace.A[5768] = acadoWorkspace.evHu[104];
-acadoWorkspace.A[5769] = acadoWorkspace.evHu[105];
-acadoWorkspace.A[5836] = acadoWorkspace.evHu[106];
-acadoWorkspace.A[5837] = acadoWorkspace.evHu[107];
-acadoWorkspace.A[5906] = acadoWorkspace.evHu[108];
-acadoWorkspace.A[5907] = acadoWorkspace.evHu[109];
-acadoWorkspace.A[5974] = acadoWorkspace.evHu[110];
-acadoWorkspace.A[5975] = acadoWorkspace.evHu[111];
-acadoWorkspace.A[6044] = acadoWorkspace.evHu[112];
-acadoWorkspace.A[6045] = acadoWorkspace.evHu[113];
-acadoWorkspace.A[6112] = acadoWorkspace.evHu[114];
-acadoWorkspace.A[6113] = acadoWorkspace.evHu[115];
-acadoWorkspace.A[6182] = acadoWorkspace.evHu[116];
-acadoWorkspace.A[6183] = acadoWorkspace.evHu[117];
-acadoWorkspace.A[6250] = acadoWorkspace.evHu[118];
-acadoWorkspace.A[6251] = acadoWorkspace.evHu[119];
-acadoWorkspace.A[6320] = acadoWorkspace.evHu[120];
-acadoWorkspace.A[6321] = acadoWorkspace.evHu[121];
-acadoWorkspace.A[6388] = acadoWorkspace.evHu[122];
-acadoWorkspace.A[6389] = acadoWorkspace.evHu[123];
-acadoWorkspace.A[6458] = acadoWorkspace.evHu[124];
-acadoWorkspace.A[6459] = acadoWorkspace.evHu[125];
-acadoWorkspace.A[6526] = acadoWorkspace.evHu[126];
-acadoWorkspace.A[6527] = acadoWorkspace.evHu[127];
-acadoWorkspace.lbA[32] = - acadoWorkspace.evH[0];
-acadoWorkspace.lbA[33] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[1];
-acadoWorkspace.lbA[34] = - acadoWorkspace.evH[2];
-acadoWorkspace.lbA[35] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[3];
-acadoWorkspace.lbA[36] = - acadoWorkspace.evH[4];
-acadoWorkspace.lbA[37] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[5];
-acadoWorkspace.lbA[38] = - acadoWorkspace.evH[6];
-acadoWorkspace.lbA[39] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[7];
-acadoWorkspace.lbA[40] = - acadoWorkspace.evH[8];
-acadoWorkspace.lbA[41] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[9];
-acadoWorkspace.lbA[42] = - acadoWorkspace.evH[10];
-acadoWorkspace.lbA[43] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[11];
-acadoWorkspace.lbA[44] = - acadoWorkspace.evH[12];
-acadoWorkspace.lbA[45] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[13];
-acadoWorkspace.lbA[46] = - acadoWorkspace.evH[14];
-acadoWorkspace.lbA[47] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[15];
-acadoWorkspace.lbA[48] = - acadoWorkspace.evH[16];
-acadoWorkspace.lbA[49] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[17];
-acadoWorkspace.lbA[50] = - acadoWorkspace.evH[18];
-acadoWorkspace.lbA[51] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[19];
-acadoWorkspace.lbA[52] = - acadoWorkspace.evH[20];
-acadoWorkspace.lbA[53] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[21];
-acadoWorkspace.lbA[54] = - acadoWorkspace.evH[22];
-acadoWorkspace.lbA[55] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[23];
-acadoWorkspace.lbA[56] = - acadoWorkspace.evH[24];
-acadoWorkspace.lbA[57] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[25];
-acadoWorkspace.lbA[58] = - acadoWorkspace.evH[26];
-acadoWorkspace.lbA[59] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[27];
-acadoWorkspace.lbA[60] = - acadoWorkspace.evH[28];
-acadoWorkspace.lbA[61] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[29];
-acadoWorkspace.lbA[62] = - acadoWorkspace.evH[30];
-acadoWorkspace.lbA[63] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[31];
-acadoWorkspace.lbA[64] = - acadoWorkspace.evH[32];
-acadoWorkspace.lbA[65] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[33];
-acadoWorkspace.lbA[66] = - acadoWorkspace.evH[34];
-acadoWorkspace.lbA[67] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[35];
-acadoWorkspace.lbA[68] = - acadoWorkspace.evH[36];
-acadoWorkspace.lbA[69] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[37];
-acadoWorkspace.lbA[70] = - acadoWorkspace.evH[38];
-acadoWorkspace.lbA[71] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[39];
-acadoWorkspace.lbA[72] = - acadoWorkspace.evH[40];
-acadoWorkspace.lbA[73] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[41];
-acadoWorkspace.lbA[74] = - acadoWorkspace.evH[42];
-acadoWorkspace.lbA[75] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[43];
-acadoWorkspace.lbA[76] = - acadoWorkspace.evH[44];
-acadoWorkspace.lbA[77] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[45];
-acadoWorkspace.lbA[78] = - acadoWorkspace.evH[46];
-acadoWorkspace.lbA[79] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[47];
-acadoWorkspace.lbA[80] = - acadoWorkspace.evH[48];
-acadoWorkspace.lbA[81] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[49];
-acadoWorkspace.lbA[82] = - acadoWorkspace.evH[50];
-acadoWorkspace.lbA[83] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[51];
-acadoWorkspace.lbA[84] = - acadoWorkspace.evH[52];
-acadoWorkspace.lbA[85] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[53];
-acadoWorkspace.lbA[86] = - acadoWorkspace.evH[54];
-acadoWorkspace.lbA[87] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[55];
-acadoWorkspace.lbA[88] = - acadoWorkspace.evH[56];
-acadoWorkspace.lbA[89] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[57];
-acadoWorkspace.lbA[90] = - acadoWorkspace.evH[58];
-acadoWorkspace.lbA[91] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[59];
-acadoWorkspace.lbA[92] = - acadoWorkspace.evH[60];
-acadoWorkspace.lbA[93] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[61];
-acadoWorkspace.lbA[94] = - acadoWorkspace.evH[62];
-acadoWorkspace.lbA[95] = (real_t)-1.0000000000000000e+12 - acadoWorkspace.evH[63];
-
-acadoWorkspace.ubA[32] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[0];
-acadoWorkspace.ubA[33] = - acadoWorkspace.evH[1];
-acadoWorkspace.ubA[34] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[2];
-acadoWorkspace.ubA[35] = - acadoWorkspace.evH[3];
-acadoWorkspace.ubA[36] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[4];
-acadoWorkspace.ubA[37] = - acadoWorkspace.evH[5];
-acadoWorkspace.ubA[38] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[6];
-acadoWorkspace.ubA[39] = - acadoWorkspace.evH[7];
-acadoWorkspace.ubA[40] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[8];
-acadoWorkspace.ubA[41] = - acadoWorkspace.evH[9];
-acadoWorkspace.ubA[42] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[10];
-acadoWorkspace.ubA[43] = - acadoWorkspace.evH[11];
-acadoWorkspace.ubA[44] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[12];
-acadoWorkspace.ubA[45] = - acadoWorkspace.evH[13];
-acadoWorkspace.ubA[46] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[14];
-acadoWorkspace.ubA[47] = - acadoWorkspace.evH[15];
-acadoWorkspace.ubA[48] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[16];
-acadoWorkspace.ubA[49] = - acadoWorkspace.evH[17];
-acadoWorkspace.ubA[50] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[18];
-acadoWorkspace.ubA[51] = - acadoWorkspace.evH[19];
-acadoWorkspace.ubA[52] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[20];
-acadoWorkspace.ubA[53] = - acadoWorkspace.evH[21];
-acadoWorkspace.ubA[54] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[22];
-acadoWorkspace.ubA[55] = - acadoWorkspace.evH[23];
-acadoWorkspace.ubA[56] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[24];
-acadoWorkspace.ubA[57] = - acadoWorkspace.evH[25];
-acadoWorkspace.ubA[58] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[26];
-acadoWorkspace.ubA[59] = - acadoWorkspace.evH[27];
-acadoWorkspace.ubA[60] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[28];
-acadoWorkspace.ubA[61] = - acadoWorkspace.evH[29];
-acadoWorkspace.ubA[62] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[30];
-acadoWorkspace.ubA[63] = - acadoWorkspace.evH[31];
-acadoWorkspace.ubA[64] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[32];
-acadoWorkspace.ubA[65] = - acadoWorkspace.evH[33];
-acadoWorkspace.ubA[66] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[34];
-acadoWorkspace.ubA[67] = - acadoWorkspace.evH[35];
-acadoWorkspace.ubA[68] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[36];
-acadoWorkspace.ubA[69] = - acadoWorkspace.evH[37];
-acadoWorkspace.ubA[70] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[38];
-acadoWorkspace.ubA[71] = - acadoWorkspace.evH[39];
-acadoWorkspace.ubA[72] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[40];
-acadoWorkspace.ubA[73] = - acadoWorkspace.evH[41];
-acadoWorkspace.ubA[74] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[42];
-acadoWorkspace.ubA[75] = - acadoWorkspace.evH[43];
-acadoWorkspace.ubA[76] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[44];
-acadoWorkspace.ubA[77] = - acadoWorkspace.evH[45];
-acadoWorkspace.ubA[78] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[46];
-acadoWorkspace.ubA[79] = - acadoWorkspace.evH[47];
-acadoWorkspace.ubA[80] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[48];
-acadoWorkspace.ubA[81] = - acadoWorkspace.evH[49];
-acadoWorkspace.ubA[82] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[50];
-acadoWorkspace.ubA[83] = - acadoWorkspace.evH[51];
-acadoWorkspace.ubA[84] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[52];
-acadoWorkspace.ubA[85] = - acadoWorkspace.evH[53];
-acadoWorkspace.ubA[86] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[54];
-acadoWorkspace.ubA[87] = - acadoWorkspace.evH[55];
-acadoWorkspace.ubA[88] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[56];
-acadoWorkspace.ubA[89] = - acadoWorkspace.evH[57];
-acadoWorkspace.ubA[90] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[58];
-acadoWorkspace.ubA[91] = - acadoWorkspace.evH[59];
-acadoWorkspace.ubA[92] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[60];
-acadoWorkspace.ubA[93] = - acadoWorkspace.evH[61];
-acadoWorkspace.ubA[94] = (real_t)1.0000000000000000e+12 - acadoWorkspace.evH[62];
-acadoWorkspace.ubA[95] = - acadoWorkspace.evH[63];
-
-acado_macHxd( &(acadoWorkspace.evHx[ 8 ]), acadoWorkspace.d, &(acadoWorkspace.lbA[ 34 ]), &(acadoWorkspace.ubA[ 34 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 16 ]), &(acadoWorkspace.d[ 4 ]), &(acadoWorkspace.lbA[ 36 ]), &(acadoWorkspace.ubA[ 36 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 24 ]), &(acadoWorkspace.d[ 8 ]), &(acadoWorkspace.lbA[ 38 ]), &(acadoWorkspace.ubA[ 38 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 32 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.lbA[ 40 ]), &(acadoWorkspace.ubA[ 40 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 40 ]), &(acadoWorkspace.d[ 16 ]), &(acadoWorkspace.lbA[ 42 ]), &(acadoWorkspace.ubA[ 42 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 48 ]), &(acadoWorkspace.d[ 20 ]), &(acadoWorkspace.lbA[ 44 ]), &(acadoWorkspace.ubA[ 44 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 56 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.lbA[ 46 ]), &(acadoWorkspace.ubA[ 46 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 64 ]), &(acadoWorkspace.d[ 28 ]), &(acadoWorkspace.lbA[ 48 ]), &(acadoWorkspace.ubA[ 48 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 72 ]), &(acadoWorkspace.d[ 32 ]), &(acadoWorkspace.lbA[ 50 ]), &(acadoWorkspace.ubA[ 50 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 80 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.lbA[ 52 ]), &(acadoWorkspace.ubA[ 52 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 88 ]), &(acadoWorkspace.d[ 40 ]), &(acadoWorkspace.lbA[ 54 ]), &(acadoWorkspace.ubA[ 54 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 96 ]), &(acadoWorkspace.d[ 44 ]), &(acadoWorkspace.lbA[ 56 ]), &(acadoWorkspace.ubA[ 56 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 104 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.lbA[ 58 ]), &(acadoWorkspace.ubA[ 58 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 112 ]), &(acadoWorkspace.d[ 52 ]), &(acadoWorkspace.lbA[ 60 ]), &(acadoWorkspace.ubA[ 60 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 120 ]), &(acadoWorkspace.d[ 56 ]), &(acadoWorkspace.lbA[ 62 ]), &(acadoWorkspace.ubA[ 62 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 128 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.lbA[ 64 ]), &(acadoWorkspace.ubA[ 64 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 136 ]), &(acadoWorkspace.d[ 64 ]), &(acadoWorkspace.lbA[ 66 ]), &(acadoWorkspace.ubA[ 66 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 144 ]), &(acadoWorkspace.d[ 68 ]), &(acadoWorkspace.lbA[ 68 ]), &(acadoWorkspace.ubA[ 68 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 152 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.lbA[ 70 ]), &(acadoWorkspace.ubA[ 70 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 160 ]), &(acadoWorkspace.d[ 76 ]), &(acadoWorkspace.lbA[ 72 ]), &(acadoWorkspace.ubA[ 72 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 168 ]), &(acadoWorkspace.d[ 80 ]), &(acadoWorkspace.lbA[ 74 ]), &(acadoWorkspace.ubA[ 74 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 176 ]), &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.lbA[ 76 ]), &(acadoWorkspace.ubA[ 76 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 184 ]), &(acadoWorkspace.d[ 88 ]), &(acadoWorkspace.lbA[ 78 ]), &(acadoWorkspace.ubA[ 78 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 192 ]), &(acadoWorkspace.d[ 92 ]), &(acadoWorkspace.lbA[ 80 ]), &(acadoWorkspace.ubA[ 80 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 200 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.lbA[ 82 ]), &(acadoWorkspace.ubA[ 82 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 208 ]), &(acadoWorkspace.d[ 100 ]), &(acadoWorkspace.lbA[ 84 ]), &(acadoWorkspace.ubA[ 84 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 216 ]), &(acadoWorkspace.d[ 104 ]), &(acadoWorkspace.lbA[ 86 ]), &(acadoWorkspace.ubA[ 86 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 224 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.lbA[ 88 ]), &(acadoWorkspace.ubA[ 88 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 232 ]), &(acadoWorkspace.d[ 112 ]), &(acadoWorkspace.lbA[ 90 ]), &(acadoWorkspace.ubA[ 90 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 240 ]), &(acadoWorkspace.d[ 116 ]), &(acadoWorkspace.lbA[ 92 ]), &(acadoWorkspace.ubA[ 92 ]) );
-acado_macHxd( &(acadoWorkspace.evHx[ 248 ]), &(acadoWorkspace.d[ 120 ]), &(acadoWorkspace.lbA[ 94 ]), &(acadoWorkspace.ubA[ 94 ]) );
-
-}
-
-void acado_condenseFdb(  )
-{
-int lRun1;
-int lRun2;
-int lRun3;
-real_t tmp;
-
-acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0];
-acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1];
-acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2];
-acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3];
-
-for (lRun2 = 0; lRun2 < 160; ++lRun2)
-acadoWorkspace.Dy[lRun2] -= acadoVariables.y[lRun2];
-
-acadoWorkspace.DyN[0] -= acadoVariables.yN[0];
-acadoWorkspace.DyN[1] -= acadoVariables.yN[1];
-acadoWorkspace.DyN[2] -= acadoVariables.yN[2];
-
-acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 4 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 10 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 30 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 50 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 70 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 80 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 90 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 100 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 110 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.g[ 26 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 120 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 28 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 130 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.g[ 30 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 140 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.g[ 32 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 150 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.g[ 34 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 160 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.g[ 36 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 170 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.g[ 38 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 180 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.g[ 40 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 190 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.g[ 42 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 200 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.g[ 44 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 210 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.g[ 46 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 220 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.g[ 48 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 230 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.g[ 50 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 240 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.g[ 52 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 250 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.g[ 54 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 260 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.g[ 56 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 270 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.g[ 58 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 280 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.g[ 60 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 290 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.g[ 62 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 300 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.g[ 64 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 310 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.g[ 66 ]) );
-
-acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy );
-acado_multQDy( &(acadoWorkspace.Q2[ 20 ]), &(acadoWorkspace.Dy[ 5 ]), &(acadoWorkspace.QDy[ 4 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 40 ]), &(acadoWorkspace.Dy[ 10 ]), &(acadoWorkspace.QDy[ 8 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 15 ]), &(acadoWorkspace.QDy[ 12 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 80 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 16 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 100 ]), &(acadoWorkspace.Dy[ 25 ]), &(acadoWorkspace.QDy[ 20 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 30 ]), &(acadoWorkspace.QDy[ 24 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 140 ]), &(acadoWorkspace.Dy[ 35 ]), &(acadoWorkspace.QDy[ 28 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 160 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 32 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 45 ]), &(acadoWorkspace.QDy[ 36 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 200 ]), &(acadoWorkspace.Dy[ 50 ]), &(acadoWorkspace.QDy[ 40 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 220 ]), &(acadoWorkspace.Dy[ 55 ]), &(acadoWorkspace.QDy[ 44 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 48 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 260 ]), &(acadoWorkspace.Dy[ 65 ]), &(acadoWorkspace.QDy[ 52 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 280 ]), &(acadoWorkspace.Dy[ 70 ]), &(acadoWorkspace.QDy[ 56 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 300 ]), &(acadoWorkspace.Dy[ 75 ]), &(acadoWorkspace.QDy[ 60 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 320 ]), &(acadoWorkspace.Dy[ 80 ]), &(acadoWorkspace.QDy[ 64 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 340 ]), &(acadoWorkspace.Dy[ 85 ]), &(acadoWorkspace.QDy[ 68 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 90 ]), &(acadoWorkspace.QDy[ 72 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 380 ]), &(acadoWorkspace.Dy[ 95 ]), &(acadoWorkspace.QDy[ 76 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 400 ]), &(acadoWorkspace.Dy[ 100 ]), &(acadoWorkspace.QDy[ 80 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 420 ]), &(acadoWorkspace.Dy[ 105 ]), &(acadoWorkspace.QDy[ 84 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 440 ]), &(acadoWorkspace.Dy[ 110 ]), &(acadoWorkspace.QDy[ 88 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 460 ]), &(acadoWorkspace.Dy[ 115 ]), &(acadoWorkspace.QDy[ 92 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 480 ]), &(acadoWorkspace.Dy[ 120 ]), &(acadoWorkspace.QDy[ 96 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 500 ]), &(acadoWorkspace.Dy[ 125 ]), &(acadoWorkspace.QDy[ 100 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 520 ]), &(acadoWorkspace.Dy[ 130 ]), &(acadoWorkspace.QDy[ 104 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 540 ]), &(acadoWorkspace.Dy[ 135 ]), &(acadoWorkspace.QDy[ 108 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 560 ]), &(acadoWorkspace.Dy[ 140 ]), &(acadoWorkspace.QDy[ 112 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 580 ]), &(acadoWorkspace.Dy[ 145 ]), &(acadoWorkspace.QDy[ 116 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 600 ]), &(acadoWorkspace.Dy[ 150 ]), &(acadoWorkspace.QDy[ 120 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 620 ]), &(acadoWorkspace.Dy[ 155 ]), &(acadoWorkspace.QDy[ 124 ]) );
-
-acadoWorkspace.QDy[128] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[129] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[130] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[131] = + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[2];
-
-for (lRun2 = 0; lRun2 < 128; ++lRun2)
-acadoWorkspace.QDy[lRun2 + 4] += acadoWorkspace.Qd[lRun2];
-
-
-acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[320]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[131];
-acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[321]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[131];
-acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[322]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[131];
-acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[323]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[125] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[126] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[127] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[128] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[129] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[130] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[131];
-
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-for (lRun2 = lRun1; lRun2 < 32; ++lRun2)
-{
-lRun3 = (((lRun2 + 1) * (lRun2)) / (2)) + (lRun1);
-acado_multEQDy( &(acadoWorkspace.E[ lRun3 * 8 ]), &(acadoWorkspace.QDy[ lRun2 * 4 + 4 ]), &(acadoWorkspace.g[ lRun1 * 2 + 4 ]) );
-}
-}
-
-acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3];
-acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0];
-acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1];
-acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3];
-tmp = acadoVariables.x[5] + acadoWorkspace.d[1];
-acadoWorkspace.lbA[0] = - tmp;
-acadoWorkspace.ubA[0] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[9] + acadoWorkspace.d[5];
-acadoWorkspace.lbA[1] = - tmp;
-acadoWorkspace.ubA[1] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[13] + acadoWorkspace.d[9];
-acadoWorkspace.lbA[2] = - tmp;
-acadoWorkspace.ubA[2] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[17] + acadoWorkspace.d[13];
-acadoWorkspace.lbA[3] = - tmp;
-acadoWorkspace.ubA[3] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[21] + acadoWorkspace.d[17];
-acadoWorkspace.lbA[4] = - tmp;
-acadoWorkspace.ubA[4] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[25] + acadoWorkspace.d[21];
-acadoWorkspace.lbA[5] = - tmp;
-acadoWorkspace.ubA[5] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[29] + acadoWorkspace.d[25];
-acadoWorkspace.lbA[6] = - tmp;
-acadoWorkspace.ubA[6] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[33] + acadoWorkspace.d[29];
-acadoWorkspace.lbA[7] = - tmp;
-acadoWorkspace.ubA[7] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[37] + acadoWorkspace.d[33];
-acadoWorkspace.lbA[8] = - tmp;
-acadoWorkspace.ubA[8] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[41] + acadoWorkspace.d[37];
-acadoWorkspace.lbA[9] = - tmp;
-acadoWorkspace.ubA[9] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[45] + acadoWorkspace.d[41];
-acadoWorkspace.lbA[10] = - tmp;
-acadoWorkspace.ubA[10] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[49] + acadoWorkspace.d[45];
-acadoWorkspace.lbA[11] = - tmp;
-acadoWorkspace.ubA[11] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[53] + acadoWorkspace.d[49];
-acadoWorkspace.lbA[12] = - tmp;
-acadoWorkspace.ubA[12] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[57] + acadoWorkspace.d[53];
-acadoWorkspace.lbA[13] = - tmp;
-acadoWorkspace.ubA[13] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[61] + acadoWorkspace.d[57];
-acadoWorkspace.lbA[14] = - tmp;
-acadoWorkspace.ubA[14] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[65] + acadoWorkspace.d[61];
-acadoWorkspace.lbA[15] = - tmp;
-acadoWorkspace.ubA[15] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[69] + acadoWorkspace.d[65];
-acadoWorkspace.lbA[16] = - tmp;
-acadoWorkspace.ubA[16] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[73] + acadoWorkspace.d[69];
-acadoWorkspace.lbA[17] = - tmp;
-acadoWorkspace.ubA[17] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[77] + acadoWorkspace.d[73];
-acadoWorkspace.lbA[18] = - tmp;
-acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[81] + acadoWorkspace.d[77];
-acadoWorkspace.lbA[19] = - tmp;
-acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[85] + acadoWorkspace.d[81];
-acadoWorkspace.lbA[20] = - tmp;
-acadoWorkspace.ubA[20] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[89] + acadoWorkspace.d[85];
-acadoWorkspace.lbA[21] = - tmp;
-acadoWorkspace.ubA[21] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[93] + acadoWorkspace.d[89];
-acadoWorkspace.lbA[22] = - tmp;
-acadoWorkspace.ubA[22] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[97] + acadoWorkspace.d[93];
-acadoWorkspace.lbA[23] = - tmp;
-acadoWorkspace.ubA[23] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[101] + acadoWorkspace.d[97];
-acadoWorkspace.lbA[24] = - tmp;
-acadoWorkspace.ubA[24] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[105] + acadoWorkspace.d[101];
-acadoWorkspace.lbA[25] = - tmp;
-acadoWorkspace.ubA[25] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[109] + acadoWorkspace.d[105];
-acadoWorkspace.lbA[26] = - tmp;
-acadoWorkspace.ubA[26] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[113] + acadoWorkspace.d[109];
-acadoWorkspace.lbA[27] = - tmp;
-acadoWorkspace.ubA[27] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[117] + acadoWorkspace.d[113];
-acadoWorkspace.lbA[28] = - tmp;
-acadoWorkspace.ubA[28] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[121] + acadoWorkspace.d[117];
-acadoWorkspace.lbA[29] = - tmp;
-acadoWorkspace.ubA[29] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[125] + acadoWorkspace.d[121];
-acadoWorkspace.lbA[30] = - tmp;
-acadoWorkspace.ubA[30] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[129] + acadoWorkspace.d[125];
-acadoWorkspace.lbA[31] = - tmp;
-acadoWorkspace.ubA[31] = (real_t)1.0000000000000000e+12 - tmp;
-
-}
-
-void acado_expand(  )
-{
-int lRun1;
-int lRun2;
-int lRun3;
-acadoVariables.x[0] += acadoWorkspace.x[0];
-acadoVariables.x[1] += acadoWorkspace.x[1];
-acadoVariables.x[2] += acadoWorkspace.x[2];
-acadoVariables.x[3] += acadoWorkspace.x[3];
-
-acadoVariables.u[0] += acadoWorkspace.x[4];
-acadoVariables.u[1] += acadoWorkspace.x[5];
-acadoVariables.u[2] += acadoWorkspace.x[6];
-acadoVariables.u[3] += acadoWorkspace.x[7];
-acadoVariables.u[4] += acadoWorkspace.x[8];
-acadoVariables.u[5] += acadoWorkspace.x[9];
-acadoVariables.u[6] += acadoWorkspace.x[10];
-acadoVariables.u[7] += acadoWorkspace.x[11];
-acadoVariables.u[8] += acadoWorkspace.x[12];
-acadoVariables.u[9] += acadoWorkspace.x[13];
-acadoVariables.u[10] += acadoWorkspace.x[14];
-acadoVariables.u[11] += acadoWorkspace.x[15];
-acadoVariables.u[12] += acadoWorkspace.x[16];
-acadoVariables.u[13] += acadoWorkspace.x[17];
-acadoVariables.u[14] += acadoWorkspace.x[18];
-acadoVariables.u[15] += acadoWorkspace.x[19];
-acadoVariables.u[16] += acadoWorkspace.x[20];
-acadoVariables.u[17] += acadoWorkspace.x[21];
-acadoVariables.u[18] += acadoWorkspace.x[22];
-acadoVariables.u[19] += acadoWorkspace.x[23];
-acadoVariables.u[20] += acadoWorkspace.x[24];
-acadoVariables.u[21] += acadoWorkspace.x[25];
-acadoVariables.u[22] += acadoWorkspace.x[26];
-acadoVariables.u[23] += acadoWorkspace.x[27];
-acadoVariables.u[24] += acadoWorkspace.x[28];
-acadoVariables.u[25] += acadoWorkspace.x[29];
-acadoVariables.u[26] += acadoWorkspace.x[30];
-acadoVariables.u[27] += acadoWorkspace.x[31];
-acadoVariables.u[28] += acadoWorkspace.x[32];
-acadoVariables.u[29] += acadoWorkspace.x[33];
-acadoVariables.u[30] += acadoWorkspace.x[34];
-acadoVariables.u[31] += acadoWorkspace.x[35];
-acadoVariables.u[32] += acadoWorkspace.x[36];
-acadoVariables.u[33] += acadoWorkspace.x[37];
-acadoVariables.u[34] += acadoWorkspace.x[38];
-acadoVariables.u[35] += acadoWorkspace.x[39];
-acadoVariables.u[36] += acadoWorkspace.x[40];
-acadoVariables.u[37] += acadoWorkspace.x[41];
-acadoVariables.u[38] += acadoWorkspace.x[42];
-acadoVariables.u[39] += acadoWorkspace.x[43];
-acadoVariables.u[40] += acadoWorkspace.x[44];
-acadoVariables.u[41] += acadoWorkspace.x[45];
-acadoVariables.u[42] += acadoWorkspace.x[46];
-acadoVariables.u[43] += acadoWorkspace.x[47];
-acadoVariables.u[44] += acadoWorkspace.x[48];
-acadoVariables.u[45] += acadoWorkspace.x[49];
-acadoVariables.u[46] += acadoWorkspace.x[50];
-acadoVariables.u[47] += acadoWorkspace.x[51];
-acadoVariables.u[48] += acadoWorkspace.x[52];
-acadoVariables.u[49] += acadoWorkspace.x[53];
-acadoVariables.u[50] += acadoWorkspace.x[54];
-acadoVariables.u[51] += acadoWorkspace.x[55];
-acadoVariables.u[52] += acadoWorkspace.x[56];
-acadoVariables.u[53] += acadoWorkspace.x[57];
-acadoVariables.u[54] += acadoWorkspace.x[58];
-acadoVariables.u[55] += acadoWorkspace.x[59];
-acadoVariables.u[56] += acadoWorkspace.x[60];
-acadoVariables.u[57] += acadoWorkspace.x[61];
-acadoVariables.u[58] += acadoWorkspace.x[62];
-acadoVariables.u[59] += acadoWorkspace.x[63];
-acadoVariables.u[60] += acadoWorkspace.x[64];
-acadoVariables.u[61] += acadoWorkspace.x[65];
-acadoVariables.u[62] += acadoWorkspace.x[66];
-acadoVariables.u[63] += acadoWorkspace.x[67];
-
-acadoVariables.x[4] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.d[0];
-acadoVariables.x[5] += + acadoWorkspace.evGx[4]*acadoWorkspace.x[0] + acadoWorkspace.evGx[5]*acadoWorkspace.x[1] + acadoWorkspace.evGx[6]*acadoWorkspace.x[2] + acadoWorkspace.evGx[7]*acadoWorkspace.x[3] + acadoWorkspace.d[1];
-acadoVariables.x[6] += + acadoWorkspace.evGx[8]*acadoWorkspace.x[0] + acadoWorkspace.evGx[9]*acadoWorkspace.x[1] + acadoWorkspace.evGx[10]*acadoWorkspace.x[2] + acadoWorkspace.evGx[11]*acadoWorkspace.x[3] + acadoWorkspace.d[2];
-acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.d[3];
-acadoVariables.x[8] += + acadoWorkspace.evGx[16]*acadoWorkspace.x[0] + acadoWorkspace.evGx[17]*acadoWorkspace.x[1] + acadoWorkspace.evGx[18]*acadoWorkspace.x[2] + acadoWorkspace.evGx[19]*acadoWorkspace.x[3] + acadoWorkspace.d[4];
-acadoVariables.x[9] += + acadoWorkspace.evGx[20]*acadoWorkspace.x[0] + acadoWorkspace.evGx[21]*acadoWorkspace.x[1] + acadoWorkspace.evGx[22]*acadoWorkspace.x[2] + acadoWorkspace.evGx[23]*acadoWorkspace.x[3] + acadoWorkspace.d[5];
-acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.d[6];
-acadoVariables.x[11] += + acadoWorkspace.evGx[28]*acadoWorkspace.x[0] + acadoWorkspace.evGx[29]*acadoWorkspace.x[1] + acadoWorkspace.evGx[30]*acadoWorkspace.x[2] + acadoWorkspace.evGx[31]*acadoWorkspace.x[3] + acadoWorkspace.d[7];
-acadoVariables.x[12] += + acadoWorkspace.evGx[32]*acadoWorkspace.x[0] + acadoWorkspace.evGx[33]*acadoWorkspace.x[1] + acadoWorkspace.evGx[34]*acadoWorkspace.x[2] + acadoWorkspace.evGx[35]*acadoWorkspace.x[3] + acadoWorkspace.d[8];
-acadoVariables.x[13] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.d[9];
-acadoVariables.x[14] += + acadoWorkspace.evGx[40]*acadoWorkspace.x[0] + acadoWorkspace.evGx[41]*acadoWorkspace.x[1] + acadoWorkspace.evGx[42]*acadoWorkspace.x[2] + acadoWorkspace.evGx[43]*acadoWorkspace.x[3] + acadoWorkspace.d[10];
-acadoVariables.x[15] += + acadoWorkspace.evGx[44]*acadoWorkspace.x[0] + acadoWorkspace.evGx[45]*acadoWorkspace.x[1] + acadoWorkspace.evGx[46]*acadoWorkspace.x[2] + acadoWorkspace.evGx[47]*acadoWorkspace.x[3] + acadoWorkspace.d[11];
-acadoVariables.x[16] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.d[12];
-acadoVariables.x[17] += + acadoWorkspace.evGx[52]*acadoWorkspace.x[0] + acadoWorkspace.evGx[53]*acadoWorkspace.x[1] + acadoWorkspace.evGx[54]*acadoWorkspace.x[2] + acadoWorkspace.evGx[55]*acadoWorkspace.x[3] + acadoWorkspace.d[13];
-acadoVariables.x[18] += + acadoWorkspace.evGx[56]*acadoWorkspace.x[0] + acadoWorkspace.evGx[57]*acadoWorkspace.x[1] + acadoWorkspace.evGx[58]*acadoWorkspace.x[2] + acadoWorkspace.evGx[59]*acadoWorkspace.x[3] + acadoWorkspace.d[14];
-acadoVariables.x[19] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.d[15];
-acadoVariables.x[20] += + acadoWorkspace.evGx[64]*acadoWorkspace.x[0] + acadoWorkspace.evGx[65]*acadoWorkspace.x[1] + acadoWorkspace.evGx[66]*acadoWorkspace.x[2] + acadoWorkspace.evGx[67]*acadoWorkspace.x[3] + acadoWorkspace.d[16];
-acadoVariables.x[21] += + acadoWorkspace.evGx[68]*acadoWorkspace.x[0] + acadoWorkspace.evGx[69]*acadoWorkspace.x[1] + acadoWorkspace.evGx[70]*acadoWorkspace.x[2] + acadoWorkspace.evGx[71]*acadoWorkspace.x[3] + acadoWorkspace.d[17];
-acadoVariables.x[22] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.d[18];
-acadoVariables.x[23] += + acadoWorkspace.evGx[76]*acadoWorkspace.x[0] + acadoWorkspace.evGx[77]*acadoWorkspace.x[1] + acadoWorkspace.evGx[78]*acadoWorkspace.x[2] + acadoWorkspace.evGx[79]*acadoWorkspace.x[3] + acadoWorkspace.d[19];
-acadoVariables.x[24] += + acadoWorkspace.evGx[80]*acadoWorkspace.x[0] + acadoWorkspace.evGx[81]*acadoWorkspace.x[1] + acadoWorkspace.evGx[82]*acadoWorkspace.x[2] + acadoWorkspace.evGx[83]*acadoWorkspace.x[3] + acadoWorkspace.d[20];
-acadoVariables.x[25] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.d[21];
-acadoVariables.x[26] += + acadoWorkspace.evGx[88]*acadoWorkspace.x[0] + acadoWorkspace.evGx[89]*acadoWorkspace.x[1] + acadoWorkspace.evGx[90]*acadoWorkspace.x[2] + acadoWorkspace.evGx[91]*acadoWorkspace.x[3] + acadoWorkspace.d[22];
-acadoVariables.x[27] += + acadoWorkspace.evGx[92]*acadoWorkspace.x[0] + acadoWorkspace.evGx[93]*acadoWorkspace.x[1] + acadoWorkspace.evGx[94]*acadoWorkspace.x[2] + acadoWorkspace.evGx[95]*acadoWorkspace.x[3] + acadoWorkspace.d[23];
-acadoVariables.x[28] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.d[24];
-acadoVariables.x[29] += + acadoWorkspace.evGx[100]*acadoWorkspace.x[0] + acadoWorkspace.evGx[101]*acadoWorkspace.x[1] + acadoWorkspace.evGx[102]*acadoWorkspace.x[2] + acadoWorkspace.evGx[103]*acadoWorkspace.x[3] + acadoWorkspace.d[25];
-acadoVariables.x[30] += + acadoWorkspace.evGx[104]*acadoWorkspace.x[0] + acadoWorkspace.evGx[105]*acadoWorkspace.x[1] + acadoWorkspace.evGx[106]*acadoWorkspace.x[2] + acadoWorkspace.evGx[107]*acadoWorkspace.x[3] + acadoWorkspace.d[26];
-acadoVariables.x[31] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.d[27];
-acadoVariables.x[32] += + acadoWorkspace.evGx[112]*acadoWorkspace.x[0] + acadoWorkspace.evGx[113]*acadoWorkspace.x[1] + acadoWorkspace.evGx[114]*acadoWorkspace.x[2] + acadoWorkspace.evGx[115]*acadoWorkspace.x[3] + acadoWorkspace.d[28];
-acadoVariables.x[33] += + acadoWorkspace.evGx[116]*acadoWorkspace.x[0] + acadoWorkspace.evGx[117]*acadoWorkspace.x[1] + acadoWorkspace.evGx[118]*acadoWorkspace.x[2] + acadoWorkspace.evGx[119]*acadoWorkspace.x[3] + acadoWorkspace.d[29];
-acadoVariables.x[34] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.d[30];
-acadoVariables.x[35] += + acadoWorkspace.evGx[124]*acadoWorkspace.x[0] + acadoWorkspace.evGx[125]*acadoWorkspace.x[1] + acadoWorkspace.evGx[126]*acadoWorkspace.x[2] + acadoWorkspace.evGx[127]*acadoWorkspace.x[3] + acadoWorkspace.d[31];
-acadoVariables.x[36] += + acadoWorkspace.evGx[128]*acadoWorkspace.x[0] + acadoWorkspace.evGx[129]*acadoWorkspace.x[1] + acadoWorkspace.evGx[130]*acadoWorkspace.x[2] + acadoWorkspace.evGx[131]*acadoWorkspace.x[3] + acadoWorkspace.d[32];
-acadoVariables.x[37] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.d[33];
-acadoVariables.x[38] += + acadoWorkspace.evGx[136]*acadoWorkspace.x[0] + acadoWorkspace.evGx[137]*acadoWorkspace.x[1] + acadoWorkspace.evGx[138]*acadoWorkspace.x[2] + acadoWorkspace.evGx[139]*acadoWorkspace.x[3] + acadoWorkspace.d[34];
-acadoVariables.x[39] += + acadoWorkspace.evGx[140]*acadoWorkspace.x[0] + acadoWorkspace.evGx[141]*acadoWorkspace.x[1] + acadoWorkspace.evGx[142]*acadoWorkspace.x[2] + acadoWorkspace.evGx[143]*acadoWorkspace.x[3] + acadoWorkspace.d[35];
-acadoVariables.x[40] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.d[36];
-acadoVariables.x[41] += + acadoWorkspace.evGx[148]*acadoWorkspace.x[0] + acadoWorkspace.evGx[149]*acadoWorkspace.x[1] + acadoWorkspace.evGx[150]*acadoWorkspace.x[2] + acadoWorkspace.evGx[151]*acadoWorkspace.x[3] + acadoWorkspace.d[37];
-acadoVariables.x[42] += + acadoWorkspace.evGx[152]*acadoWorkspace.x[0] + acadoWorkspace.evGx[153]*acadoWorkspace.x[1] + acadoWorkspace.evGx[154]*acadoWorkspace.x[2] + acadoWorkspace.evGx[155]*acadoWorkspace.x[3] + acadoWorkspace.d[38];
-acadoVariables.x[43] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.d[39];
-acadoVariables.x[44] += + acadoWorkspace.evGx[160]*acadoWorkspace.x[0] + acadoWorkspace.evGx[161]*acadoWorkspace.x[1] + acadoWorkspace.evGx[162]*acadoWorkspace.x[2] + acadoWorkspace.evGx[163]*acadoWorkspace.x[3] + acadoWorkspace.d[40];
-acadoVariables.x[45] += + acadoWorkspace.evGx[164]*acadoWorkspace.x[0] + acadoWorkspace.evGx[165]*acadoWorkspace.x[1] + acadoWorkspace.evGx[166]*acadoWorkspace.x[2] + acadoWorkspace.evGx[167]*acadoWorkspace.x[3] + acadoWorkspace.d[41];
-acadoVariables.x[46] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.d[42];
-acadoVariables.x[47] += + acadoWorkspace.evGx[172]*acadoWorkspace.x[0] + acadoWorkspace.evGx[173]*acadoWorkspace.x[1] + acadoWorkspace.evGx[174]*acadoWorkspace.x[2] + acadoWorkspace.evGx[175]*acadoWorkspace.x[3] + acadoWorkspace.d[43];
-acadoVariables.x[48] += + acadoWorkspace.evGx[176]*acadoWorkspace.x[0] + acadoWorkspace.evGx[177]*acadoWorkspace.x[1] + acadoWorkspace.evGx[178]*acadoWorkspace.x[2] + acadoWorkspace.evGx[179]*acadoWorkspace.x[3] + acadoWorkspace.d[44];
-acadoVariables.x[49] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.d[45];
-acadoVariables.x[50] += + acadoWorkspace.evGx[184]*acadoWorkspace.x[0] + acadoWorkspace.evGx[185]*acadoWorkspace.x[1] + acadoWorkspace.evGx[186]*acadoWorkspace.x[2] + acadoWorkspace.evGx[187]*acadoWorkspace.x[3] + acadoWorkspace.d[46];
-acadoVariables.x[51] += + acadoWorkspace.evGx[188]*acadoWorkspace.x[0] + acadoWorkspace.evGx[189]*acadoWorkspace.x[1] + acadoWorkspace.evGx[190]*acadoWorkspace.x[2] + acadoWorkspace.evGx[191]*acadoWorkspace.x[3] + acadoWorkspace.d[47];
-acadoVariables.x[52] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.d[48];
-acadoVariables.x[53] += + acadoWorkspace.evGx[196]*acadoWorkspace.x[0] + acadoWorkspace.evGx[197]*acadoWorkspace.x[1] + acadoWorkspace.evGx[198]*acadoWorkspace.x[2] + acadoWorkspace.evGx[199]*acadoWorkspace.x[3] + acadoWorkspace.d[49];
-acadoVariables.x[54] += + acadoWorkspace.evGx[200]*acadoWorkspace.x[0] + acadoWorkspace.evGx[201]*acadoWorkspace.x[1] + acadoWorkspace.evGx[202]*acadoWorkspace.x[2] + acadoWorkspace.evGx[203]*acadoWorkspace.x[3] + acadoWorkspace.d[50];
-acadoVariables.x[55] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.d[51];
-acadoVariables.x[56] += + acadoWorkspace.evGx[208]*acadoWorkspace.x[0] + acadoWorkspace.evGx[209]*acadoWorkspace.x[1] + acadoWorkspace.evGx[210]*acadoWorkspace.x[2] + acadoWorkspace.evGx[211]*acadoWorkspace.x[3] + acadoWorkspace.d[52];
-acadoVariables.x[57] += + acadoWorkspace.evGx[212]*acadoWorkspace.x[0] + acadoWorkspace.evGx[213]*acadoWorkspace.x[1] + acadoWorkspace.evGx[214]*acadoWorkspace.x[2] + acadoWorkspace.evGx[215]*acadoWorkspace.x[3] + acadoWorkspace.d[53];
-acadoVariables.x[58] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.d[54];
-acadoVariables.x[59] += + acadoWorkspace.evGx[220]*acadoWorkspace.x[0] + acadoWorkspace.evGx[221]*acadoWorkspace.x[1] + acadoWorkspace.evGx[222]*acadoWorkspace.x[2] + acadoWorkspace.evGx[223]*acadoWorkspace.x[3] + acadoWorkspace.d[55];
-acadoVariables.x[60] += + acadoWorkspace.evGx[224]*acadoWorkspace.x[0] + acadoWorkspace.evGx[225]*acadoWorkspace.x[1] + acadoWorkspace.evGx[226]*acadoWorkspace.x[2] + acadoWorkspace.evGx[227]*acadoWorkspace.x[3] + acadoWorkspace.d[56];
-acadoVariables.x[61] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.d[57];
-acadoVariables.x[62] += + acadoWorkspace.evGx[232]*acadoWorkspace.x[0] + acadoWorkspace.evGx[233]*acadoWorkspace.x[1] + acadoWorkspace.evGx[234]*acadoWorkspace.x[2] + acadoWorkspace.evGx[235]*acadoWorkspace.x[3] + acadoWorkspace.d[58];
-acadoVariables.x[63] += + acadoWorkspace.evGx[236]*acadoWorkspace.x[0] + acadoWorkspace.evGx[237]*acadoWorkspace.x[1] + acadoWorkspace.evGx[238]*acadoWorkspace.x[2] + acadoWorkspace.evGx[239]*acadoWorkspace.x[3] + acadoWorkspace.d[59];
-acadoVariables.x[64] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.d[60];
-acadoVariables.x[65] += + acadoWorkspace.evGx[244]*acadoWorkspace.x[0] + acadoWorkspace.evGx[245]*acadoWorkspace.x[1] + acadoWorkspace.evGx[246]*acadoWorkspace.x[2] + acadoWorkspace.evGx[247]*acadoWorkspace.x[3] + acadoWorkspace.d[61];
-acadoVariables.x[66] += + acadoWorkspace.evGx[248]*acadoWorkspace.x[0] + acadoWorkspace.evGx[249]*acadoWorkspace.x[1] + acadoWorkspace.evGx[250]*acadoWorkspace.x[2] + acadoWorkspace.evGx[251]*acadoWorkspace.x[3] + acadoWorkspace.d[62];
-acadoVariables.x[67] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.d[63];
-acadoVariables.x[68] += + acadoWorkspace.evGx[256]*acadoWorkspace.x[0] + acadoWorkspace.evGx[257]*acadoWorkspace.x[1] + acadoWorkspace.evGx[258]*acadoWorkspace.x[2] + acadoWorkspace.evGx[259]*acadoWorkspace.x[3] + acadoWorkspace.d[64];
-acadoVariables.x[69] += + acadoWorkspace.evGx[260]*acadoWorkspace.x[0] + acadoWorkspace.evGx[261]*acadoWorkspace.x[1] + acadoWorkspace.evGx[262]*acadoWorkspace.x[2] + acadoWorkspace.evGx[263]*acadoWorkspace.x[3] + acadoWorkspace.d[65];
-acadoVariables.x[70] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.d[66];
-acadoVariables.x[71] += + acadoWorkspace.evGx[268]*acadoWorkspace.x[0] + acadoWorkspace.evGx[269]*acadoWorkspace.x[1] + acadoWorkspace.evGx[270]*acadoWorkspace.x[2] + acadoWorkspace.evGx[271]*acadoWorkspace.x[3] + acadoWorkspace.d[67];
-acadoVariables.x[72] += + acadoWorkspace.evGx[272]*acadoWorkspace.x[0] + acadoWorkspace.evGx[273]*acadoWorkspace.x[1] + acadoWorkspace.evGx[274]*acadoWorkspace.x[2] + acadoWorkspace.evGx[275]*acadoWorkspace.x[3] + acadoWorkspace.d[68];
-acadoVariables.x[73] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.d[69];
-acadoVariables.x[74] += + acadoWorkspace.evGx[280]*acadoWorkspace.x[0] + acadoWorkspace.evGx[281]*acadoWorkspace.x[1] + acadoWorkspace.evGx[282]*acadoWorkspace.x[2] + acadoWorkspace.evGx[283]*acadoWorkspace.x[3] + acadoWorkspace.d[70];
-acadoVariables.x[75] += + acadoWorkspace.evGx[284]*acadoWorkspace.x[0] + acadoWorkspace.evGx[285]*acadoWorkspace.x[1] + acadoWorkspace.evGx[286]*acadoWorkspace.x[2] + acadoWorkspace.evGx[287]*acadoWorkspace.x[3] + acadoWorkspace.d[71];
-acadoVariables.x[76] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.d[72];
-acadoVariables.x[77] += + acadoWorkspace.evGx[292]*acadoWorkspace.x[0] + acadoWorkspace.evGx[293]*acadoWorkspace.x[1] + acadoWorkspace.evGx[294]*acadoWorkspace.x[2] + acadoWorkspace.evGx[295]*acadoWorkspace.x[3] + acadoWorkspace.d[73];
-acadoVariables.x[78] += + acadoWorkspace.evGx[296]*acadoWorkspace.x[0] + acadoWorkspace.evGx[297]*acadoWorkspace.x[1] + acadoWorkspace.evGx[298]*acadoWorkspace.x[2] + acadoWorkspace.evGx[299]*acadoWorkspace.x[3] + acadoWorkspace.d[74];
-acadoVariables.x[79] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.d[75];
-acadoVariables.x[80] += + acadoWorkspace.evGx[304]*acadoWorkspace.x[0] + acadoWorkspace.evGx[305]*acadoWorkspace.x[1] + acadoWorkspace.evGx[306]*acadoWorkspace.x[2] + acadoWorkspace.evGx[307]*acadoWorkspace.x[3] + acadoWorkspace.d[76];
-acadoVariables.x[81] += + acadoWorkspace.evGx[308]*acadoWorkspace.x[0] + acadoWorkspace.evGx[309]*acadoWorkspace.x[1] + acadoWorkspace.evGx[310]*acadoWorkspace.x[2] + acadoWorkspace.evGx[311]*acadoWorkspace.x[3] + acadoWorkspace.d[77];
-acadoVariables.x[82] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.d[78];
-acadoVariables.x[83] += + acadoWorkspace.evGx[316]*acadoWorkspace.x[0] + acadoWorkspace.evGx[317]*acadoWorkspace.x[1] + acadoWorkspace.evGx[318]*acadoWorkspace.x[2] + acadoWorkspace.evGx[319]*acadoWorkspace.x[3] + acadoWorkspace.d[79];
-acadoVariables.x[84] += + acadoWorkspace.evGx[320]*acadoWorkspace.x[0] + acadoWorkspace.evGx[321]*acadoWorkspace.x[1] + acadoWorkspace.evGx[322]*acadoWorkspace.x[2] + acadoWorkspace.evGx[323]*acadoWorkspace.x[3] + acadoWorkspace.d[80];
-acadoVariables.x[85] += + acadoWorkspace.evGx[324]*acadoWorkspace.x[0] + acadoWorkspace.evGx[325]*acadoWorkspace.x[1] + acadoWorkspace.evGx[326]*acadoWorkspace.x[2] + acadoWorkspace.evGx[327]*acadoWorkspace.x[3] + acadoWorkspace.d[81];
-acadoVariables.x[86] += + acadoWorkspace.evGx[328]*acadoWorkspace.x[0] + acadoWorkspace.evGx[329]*acadoWorkspace.x[1] + acadoWorkspace.evGx[330]*acadoWorkspace.x[2] + acadoWorkspace.evGx[331]*acadoWorkspace.x[3] + acadoWorkspace.d[82];
-acadoVariables.x[87] += + acadoWorkspace.evGx[332]*acadoWorkspace.x[0] + acadoWorkspace.evGx[333]*acadoWorkspace.x[1] + acadoWorkspace.evGx[334]*acadoWorkspace.x[2] + acadoWorkspace.evGx[335]*acadoWorkspace.x[3] + acadoWorkspace.d[83];
-acadoVariables.x[88] += + acadoWorkspace.evGx[336]*acadoWorkspace.x[0] + acadoWorkspace.evGx[337]*acadoWorkspace.x[1] + acadoWorkspace.evGx[338]*acadoWorkspace.x[2] + acadoWorkspace.evGx[339]*acadoWorkspace.x[3] + acadoWorkspace.d[84];
-acadoVariables.x[89] += + acadoWorkspace.evGx[340]*acadoWorkspace.x[0] + acadoWorkspace.evGx[341]*acadoWorkspace.x[1] + acadoWorkspace.evGx[342]*acadoWorkspace.x[2] + acadoWorkspace.evGx[343]*acadoWorkspace.x[3] + acadoWorkspace.d[85];
-acadoVariables.x[90] += + acadoWorkspace.evGx[344]*acadoWorkspace.x[0] + acadoWorkspace.evGx[345]*acadoWorkspace.x[1] + acadoWorkspace.evGx[346]*acadoWorkspace.x[2] + acadoWorkspace.evGx[347]*acadoWorkspace.x[3] + acadoWorkspace.d[86];
-acadoVariables.x[91] += + acadoWorkspace.evGx[348]*acadoWorkspace.x[0] + acadoWorkspace.evGx[349]*acadoWorkspace.x[1] + acadoWorkspace.evGx[350]*acadoWorkspace.x[2] + acadoWorkspace.evGx[351]*acadoWorkspace.x[3] + acadoWorkspace.d[87];
-acadoVariables.x[92] += + acadoWorkspace.evGx[352]*acadoWorkspace.x[0] + acadoWorkspace.evGx[353]*acadoWorkspace.x[1] + acadoWorkspace.evGx[354]*acadoWorkspace.x[2] + acadoWorkspace.evGx[355]*acadoWorkspace.x[3] + acadoWorkspace.d[88];
-acadoVariables.x[93] += + acadoWorkspace.evGx[356]*acadoWorkspace.x[0] + acadoWorkspace.evGx[357]*acadoWorkspace.x[1] + acadoWorkspace.evGx[358]*acadoWorkspace.x[2] + acadoWorkspace.evGx[359]*acadoWorkspace.x[3] + acadoWorkspace.d[89];
-acadoVariables.x[94] += + acadoWorkspace.evGx[360]*acadoWorkspace.x[0] + acadoWorkspace.evGx[361]*acadoWorkspace.x[1] + acadoWorkspace.evGx[362]*acadoWorkspace.x[2] + acadoWorkspace.evGx[363]*acadoWorkspace.x[3] + acadoWorkspace.d[90];
-acadoVariables.x[95] += + acadoWorkspace.evGx[364]*acadoWorkspace.x[0] + acadoWorkspace.evGx[365]*acadoWorkspace.x[1] + acadoWorkspace.evGx[366]*acadoWorkspace.x[2] + acadoWorkspace.evGx[367]*acadoWorkspace.x[3] + acadoWorkspace.d[91];
-acadoVariables.x[96] += + acadoWorkspace.evGx[368]*acadoWorkspace.x[0] + acadoWorkspace.evGx[369]*acadoWorkspace.x[1] + acadoWorkspace.evGx[370]*acadoWorkspace.x[2] + acadoWorkspace.evGx[371]*acadoWorkspace.x[3] + acadoWorkspace.d[92];
-acadoVariables.x[97] += + acadoWorkspace.evGx[372]*acadoWorkspace.x[0] + acadoWorkspace.evGx[373]*acadoWorkspace.x[1] + acadoWorkspace.evGx[374]*acadoWorkspace.x[2] + acadoWorkspace.evGx[375]*acadoWorkspace.x[3] + acadoWorkspace.d[93];
-acadoVariables.x[98] += + acadoWorkspace.evGx[376]*acadoWorkspace.x[0] + acadoWorkspace.evGx[377]*acadoWorkspace.x[1] + acadoWorkspace.evGx[378]*acadoWorkspace.x[2] + acadoWorkspace.evGx[379]*acadoWorkspace.x[3] + acadoWorkspace.d[94];
-acadoVariables.x[99] += + acadoWorkspace.evGx[380]*acadoWorkspace.x[0] + acadoWorkspace.evGx[381]*acadoWorkspace.x[1] + acadoWorkspace.evGx[382]*acadoWorkspace.x[2] + acadoWorkspace.evGx[383]*acadoWorkspace.x[3] + acadoWorkspace.d[95];
-acadoVariables.x[100] += + acadoWorkspace.evGx[384]*acadoWorkspace.x[0] + acadoWorkspace.evGx[385]*acadoWorkspace.x[1] + acadoWorkspace.evGx[386]*acadoWorkspace.x[2] + acadoWorkspace.evGx[387]*acadoWorkspace.x[3] + acadoWorkspace.d[96];
-acadoVariables.x[101] += + acadoWorkspace.evGx[388]*acadoWorkspace.x[0] + acadoWorkspace.evGx[389]*acadoWorkspace.x[1] + acadoWorkspace.evGx[390]*acadoWorkspace.x[2] + acadoWorkspace.evGx[391]*acadoWorkspace.x[3] + acadoWorkspace.d[97];
-acadoVariables.x[102] += + acadoWorkspace.evGx[392]*acadoWorkspace.x[0] + acadoWorkspace.evGx[393]*acadoWorkspace.x[1] + acadoWorkspace.evGx[394]*acadoWorkspace.x[2] + acadoWorkspace.evGx[395]*acadoWorkspace.x[3] + acadoWorkspace.d[98];
-acadoVariables.x[103] += + acadoWorkspace.evGx[396]*acadoWorkspace.x[0] + acadoWorkspace.evGx[397]*acadoWorkspace.x[1] + acadoWorkspace.evGx[398]*acadoWorkspace.x[2] + acadoWorkspace.evGx[399]*acadoWorkspace.x[3] + acadoWorkspace.d[99];
-acadoVariables.x[104] += + acadoWorkspace.evGx[400]*acadoWorkspace.x[0] + acadoWorkspace.evGx[401]*acadoWorkspace.x[1] + acadoWorkspace.evGx[402]*acadoWorkspace.x[2] + acadoWorkspace.evGx[403]*acadoWorkspace.x[3] + acadoWorkspace.d[100];
-acadoVariables.x[105] += + acadoWorkspace.evGx[404]*acadoWorkspace.x[0] + acadoWorkspace.evGx[405]*acadoWorkspace.x[1] + acadoWorkspace.evGx[406]*acadoWorkspace.x[2] + acadoWorkspace.evGx[407]*acadoWorkspace.x[3] + acadoWorkspace.d[101];
-acadoVariables.x[106] += + acadoWorkspace.evGx[408]*acadoWorkspace.x[0] + acadoWorkspace.evGx[409]*acadoWorkspace.x[1] + acadoWorkspace.evGx[410]*acadoWorkspace.x[2] + acadoWorkspace.evGx[411]*acadoWorkspace.x[3] + acadoWorkspace.d[102];
-acadoVariables.x[107] += + acadoWorkspace.evGx[412]*acadoWorkspace.x[0] + acadoWorkspace.evGx[413]*acadoWorkspace.x[1] + acadoWorkspace.evGx[414]*acadoWorkspace.x[2] + acadoWorkspace.evGx[415]*acadoWorkspace.x[3] + acadoWorkspace.d[103];
-acadoVariables.x[108] += + acadoWorkspace.evGx[416]*acadoWorkspace.x[0] + acadoWorkspace.evGx[417]*acadoWorkspace.x[1] + acadoWorkspace.evGx[418]*acadoWorkspace.x[2] + acadoWorkspace.evGx[419]*acadoWorkspace.x[3] + acadoWorkspace.d[104];
-acadoVariables.x[109] += + acadoWorkspace.evGx[420]*acadoWorkspace.x[0] + acadoWorkspace.evGx[421]*acadoWorkspace.x[1] + acadoWorkspace.evGx[422]*acadoWorkspace.x[2] + acadoWorkspace.evGx[423]*acadoWorkspace.x[3] + acadoWorkspace.d[105];
-acadoVariables.x[110] += + acadoWorkspace.evGx[424]*acadoWorkspace.x[0] + acadoWorkspace.evGx[425]*acadoWorkspace.x[1] + acadoWorkspace.evGx[426]*acadoWorkspace.x[2] + acadoWorkspace.evGx[427]*acadoWorkspace.x[3] + acadoWorkspace.d[106];
-acadoVariables.x[111] += + acadoWorkspace.evGx[428]*acadoWorkspace.x[0] + acadoWorkspace.evGx[429]*acadoWorkspace.x[1] + acadoWorkspace.evGx[430]*acadoWorkspace.x[2] + acadoWorkspace.evGx[431]*acadoWorkspace.x[3] + acadoWorkspace.d[107];
-acadoVariables.x[112] += + acadoWorkspace.evGx[432]*acadoWorkspace.x[0] + acadoWorkspace.evGx[433]*acadoWorkspace.x[1] + acadoWorkspace.evGx[434]*acadoWorkspace.x[2] + acadoWorkspace.evGx[435]*acadoWorkspace.x[3] + acadoWorkspace.d[108];
-acadoVariables.x[113] += + acadoWorkspace.evGx[436]*acadoWorkspace.x[0] + acadoWorkspace.evGx[437]*acadoWorkspace.x[1] + acadoWorkspace.evGx[438]*acadoWorkspace.x[2] + acadoWorkspace.evGx[439]*acadoWorkspace.x[3] + acadoWorkspace.d[109];
-acadoVariables.x[114] += + acadoWorkspace.evGx[440]*acadoWorkspace.x[0] + acadoWorkspace.evGx[441]*acadoWorkspace.x[1] + acadoWorkspace.evGx[442]*acadoWorkspace.x[2] + acadoWorkspace.evGx[443]*acadoWorkspace.x[3] + acadoWorkspace.d[110];
-acadoVariables.x[115] += + acadoWorkspace.evGx[444]*acadoWorkspace.x[0] + acadoWorkspace.evGx[445]*acadoWorkspace.x[1] + acadoWorkspace.evGx[446]*acadoWorkspace.x[2] + acadoWorkspace.evGx[447]*acadoWorkspace.x[3] + acadoWorkspace.d[111];
-acadoVariables.x[116] += + acadoWorkspace.evGx[448]*acadoWorkspace.x[0] + acadoWorkspace.evGx[449]*acadoWorkspace.x[1] + acadoWorkspace.evGx[450]*acadoWorkspace.x[2] + acadoWorkspace.evGx[451]*acadoWorkspace.x[3] + acadoWorkspace.d[112];
-acadoVariables.x[117] += + acadoWorkspace.evGx[452]*acadoWorkspace.x[0] + acadoWorkspace.evGx[453]*acadoWorkspace.x[1] + acadoWorkspace.evGx[454]*acadoWorkspace.x[2] + acadoWorkspace.evGx[455]*acadoWorkspace.x[3] + acadoWorkspace.d[113];
-acadoVariables.x[118] += + acadoWorkspace.evGx[456]*acadoWorkspace.x[0] + acadoWorkspace.evGx[457]*acadoWorkspace.x[1] + acadoWorkspace.evGx[458]*acadoWorkspace.x[2] + acadoWorkspace.evGx[459]*acadoWorkspace.x[3] + acadoWorkspace.d[114];
-acadoVariables.x[119] += + acadoWorkspace.evGx[460]*acadoWorkspace.x[0] + acadoWorkspace.evGx[461]*acadoWorkspace.x[1] + acadoWorkspace.evGx[462]*acadoWorkspace.x[2] + acadoWorkspace.evGx[463]*acadoWorkspace.x[3] + acadoWorkspace.d[115];
-acadoVariables.x[120] += + acadoWorkspace.evGx[464]*acadoWorkspace.x[0] + acadoWorkspace.evGx[465]*acadoWorkspace.x[1] + acadoWorkspace.evGx[466]*acadoWorkspace.x[2] + acadoWorkspace.evGx[467]*acadoWorkspace.x[3] + acadoWorkspace.d[116];
-acadoVariables.x[121] += + acadoWorkspace.evGx[468]*acadoWorkspace.x[0] + acadoWorkspace.evGx[469]*acadoWorkspace.x[1] + acadoWorkspace.evGx[470]*acadoWorkspace.x[2] + acadoWorkspace.evGx[471]*acadoWorkspace.x[3] + acadoWorkspace.d[117];
-acadoVariables.x[122] += + acadoWorkspace.evGx[472]*acadoWorkspace.x[0] + acadoWorkspace.evGx[473]*acadoWorkspace.x[1] + acadoWorkspace.evGx[474]*acadoWorkspace.x[2] + acadoWorkspace.evGx[475]*acadoWorkspace.x[3] + acadoWorkspace.d[118];
-acadoVariables.x[123] += + acadoWorkspace.evGx[476]*acadoWorkspace.x[0] + acadoWorkspace.evGx[477]*acadoWorkspace.x[1] + acadoWorkspace.evGx[478]*acadoWorkspace.x[2] + acadoWorkspace.evGx[479]*acadoWorkspace.x[3] + acadoWorkspace.d[119];
-acadoVariables.x[124] += + acadoWorkspace.evGx[480]*acadoWorkspace.x[0] + acadoWorkspace.evGx[481]*acadoWorkspace.x[1] + acadoWorkspace.evGx[482]*acadoWorkspace.x[2] + acadoWorkspace.evGx[483]*acadoWorkspace.x[3] + acadoWorkspace.d[120];
-acadoVariables.x[125] += + acadoWorkspace.evGx[484]*acadoWorkspace.x[0] + acadoWorkspace.evGx[485]*acadoWorkspace.x[1] + acadoWorkspace.evGx[486]*acadoWorkspace.x[2] + acadoWorkspace.evGx[487]*acadoWorkspace.x[3] + acadoWorkspace.d[121];
-acadoVariables.x[126] += + acadoWorkspace.evGx[488]*acadoWorkspace.x[0] + acadoWorkspace.evGx[489]*acadoWorkspace.x[1] + acadoWorkspace.evGx[490]*acadoWorkspace.x[2] + acadoWorkspace.evGx[491]*acadoWorkspace.x[3] + acadoWorkspace.d[122];
-acadoVariables.x[127] += + acadoWorkspace.evGx[492]*acadoWorkspace.x[0] + acadoWorkspace.evGx[493]*acadoWorkspace.x[1] + acadoWorkspace.evGx[494]*acadoWorkspace.x[2] + acadoWorkspace.evGx[495]*acadoWorkspace.x[3] + acadoWorkspace.d[123];
-acadoVariables.x[128] += + acadoWorkspace.evGx[496]*acadoWorkspace.x[0] + acadoWorkspace.evGx[497]*acadoWorkspace.x[1] + acadoWorkspace.evGx[498]*acadoWorkspace.x[2] + acadoWorkspace.evGx[499]*acadoWorkspace.x[3] + acadoWorkspace.d[124];
-acadoVariables.x[129] += + acadoWorkspace.evGx[500]*acadoWorkspace.x[0] + acadoWorkspace.evGx[501]*acadoWorkspace.x[1] + acadoWorkspace.evGx[502]*acadoWorkspace.x[2] + acadoWorkspace.evGx[503]*acadoWorkspace.x[3] + acadoWorkspace.d[125];
-acadoVariables.x[130] += + acadoWorkspace.evGx[504]*acadoWorkspace.x[0] + acadoWorkspace.evGx[505]*acadoWorkspace.x[1] + acadoWorkspace.evGx[506]*acadoWorkspace.x[2] + acadoWorkspace.evGx[507]*acadoWorkspace.x[3] + acadoWorkspace.d[126];
-acadoVariables.x[131] += + acadoWorkspace.evGx[508]*acadoWorkspace.x[0] + acadoWorkspace.evGx[509]*acadoWorkspace.x[1] + acadoWorkspace.evGx[510]*acadoWorkspace.x[2] + acadoWorkspace.evGx[511]*acadoWorkspace.x[3] + acadoWorkspace.d[127];
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-for (lRun2 = 0; lRun2 < lRun1 + 1; ++lRun2)
-{
-lRun3 = (((lRun1 + 1) * (lRun1)) / (2)) + (lRun2);
-acado_multEDu( &(acadoWorkspace.E[ lRun3 * 8 ]), &(acadoWorkspace.x[ lRun2 * 2 + 4 ]), &(acadoVariables.x[ lRun1 * 4 + 4 ]) );
-}
-}
-}
-
-int acado_preparationStep(  )
-{
-int ret;
-
-ret = acado_modelSimulation();
-acado_evaluateObjective(  );
-acado_condensePrep(  );
-return ret;
-}
-
-int acado_feedbackStep(  )
-{
-int tmp;
-
-acado_condenseFdb(  );
-
-tmp = acado_solve( );
-
-acado_expand(  );
-return tmp;
-}
-
-int acado_initializeSolver(  )
-{
-int ret;
-
-/* This is a function which must be called once before any other function call! */
-
-
-ret = 0;
-
-memset(&acadoWorkspace, 0, sizeof( acadoWorkspace ));
-return ret;
-}
-
-void acado_initializeNodesByForwardSimulation(  )
-{
-int index;
-for (index = 0; index < 32; ++index)
-{
-acadoWorkspace.state[0] = acadoVariables.x[index * 4];
-acadoWorkspace.state[1] = acadoVariables.x[index * 4 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[index * 4 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[index * 4 + 3];
-acadoWorkspace.state[28] = acadoVariables.u[index * 2];
-acadoWorkspace.state[29] = acadoVariables.u[index * 2 + 1];
-acadoWorkspace.state[30] = acadoVariables.od[index * 2];
-acadoWorkspace.state[31] = acadoVariables.od[index * 2 + 1];
-
-acado_integrate(acadoWorkspace.state, index == 0, index);
-
-acadoVariables.x[index * 4 + 4] = acadoWorkspace.state[0];
-acadoVariables.x[index * 4 + 5] = acadoWorkspace.state[1];
-acadoVariables.x[index * 4 + 6] = acadoWorkspace.state[2];
-acadoVariables.x[index * 4 + 7] = acadoWorkspace.state[3];
-}
-}
-
-void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 32; ++index)
-{
-acadoVariables.x[index * 4] = acadoVariables.x[index * 4 + 4];
-acadoVariables.x[index * 4 + 1] = acadoVariables.x[index * 4 + 5];
-acadoVariables.x[index * 4 + 2] = acadoVariables.x[index * 4 + 6];
-acadoVariables.x[index * 4 + 3] = acadoVariables.x[index * 4 + 7];
-}
-
-if (strategy == 1 && xEnd != 0)
-{
-acadoVariables.x[128] = xEnd[0];
-acadoVariables.x[129] = xEnd[1];
-acadoVariables.x[130] = xEnd[2];
-acadoVariables.x[131] = xEnd[3];
-}
-else if (strategy == 2) 
-{
-acadoWorkspace.state[0] = acadoVariables.x[128];
-acadoWorkspace.state[1] = acadoVariables.x[129];
-acadoWorkspace.state[2] = acadoVariables.x[130];
-acadoWorkspace.state[3] = acadoVariables.x[131];
-if (uEnd != 0)
-{
-acadoWorkspace.state[28] = uEnd[0];
-acadoWorkspace.state[29] = uEnd[1];
-}
-else
-{
-acadoWorkspace.state[28] = acadoVariables.u[62];
-acadoWorkspace.state[29] = acadoVariables.u[63];
-}
-acadoWorkspace.state[30] = acadoVariables.od[64];
-acadoWorkspace.state[31] = acadoVariables.od[65];
-
-acado_integrate(acadoWorkspace.state, 1, 31);
-
-acadoVariables.x[128] = acadoWorkspace.state[0];
-acadoVariables.x[129] = acadoWorkspace.state[1];
-acadoVariables.x[130] = acadoWorkspace.state[2];
-acadoVariables.x[131] = acadoWorkspace.state[3];
-}
-}
-
-void acado_shiftControls( real_t* const uEnd )
-{
-int index;
-for (index = 0; index < 31; ++index)
-{
-acadoVariables.u[index * 2] = acadoVariables.u[index * 2 + 2];
-acadoVariables.u[index * 2 + 1] = acadoVariables.u[index * 2 + 3];
-}
-
-if (uEnd != 0)
-{
-acadoVariables.u[62] = uEnd[0];
-acadoVariables.u[63] = uEnd[1];
-}
-}
-
-real_t acado_getKKT(  )
-{
-real_t kkt;
-
-int index;
-real_t prd;
-
-kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25] + acadoWorkspace.g[26]*acadoWorkspace.x[26] + acadoWorkspace.g[27]*acadoWorkspace.x[27] + acadoWorkspace.g[28]*acadoWorkspace.x[28] + acadoWorkspace.g[29]*acadoWorkspace.x[29] + acadoWorkspace.g[30]*acadoWorkspace.x[30] + acadoWorkspace.g[31]*acadoWorkspace.x[31] + acadoWorkspace.g[32]*acadoWorkspace.x[32] + acadoWorkspace.g[33]*acadoWorkspace.x[33] + acadoWorkspace.g[34]*acadoWorkspace.x[34] + acadoWorkspace.g[35]*acadoWorkspace.x[35] + acadoWorkspace.g[36]*acadoWorkspace.x[36] + acadoWorkspace.g[37]*acadoWorkspace.x[37] + acadoWorkspace.g[38]*acadoWorkspace.x[38] + acadoWorkspace.g[39]*acadoWorkspace.x[39] + acadoWorkspace.g[40]*acadoWorkspace.x[40] + acadoWorkspace.g[41]*acadoWorkspace.x[41] + acadoWorkspace.g[42]*acadoWorkspace.x[42] + acadoWorkspace.g[43]*acadoWorkspace.x[43] + acadoWorkspace.g[44]*acadoWorkspace.x[44] + acadoWorkspace.g[45]*acadoWorkspace.x[45] + acadoWorkspace.g[46]*acadoWorkspace.x[46] + acadoWorkspace.g[47]*acadoWorkspace.x[47] + acadoWorkspace.g[48]*acadoWorkspace.x[48] + acadoWorkspace.g[49]*acadoWorkspace.x[49] + acadoWorkspace.g[50]*acadoWorkspace.x[50] + acadoWorkspace.g[51]*acadoWorkspace.x[51] + acadoWorkspace.g[52]*acadoWorkspace.x[52] + acadoWorkspace.g[53]*acadoWorkspace.x[53] + acadoWorkspace.g[54]*acadoWorkspace.x[54] + acadoWorkspace.g[55]*acadoWorkspace.x[55] + acadoWorkspace.g[56]*acadoWorkspace.x[56] + acadoWorkspace.g[57]*acadoWorkspace.x[57] + acadoWorkspace.g[58]*acadoWorkspace.x[58] + acadoWorkspace.g[59]*acadoWorkspace.x[59] + acadoWorkspace.g[60]*acadoWorkspace.x[60] + acadoWorkspace.g[61]*acadoWorkspace.x[61] + acadoWorkspace.g[62]*acadoWorkspace.x[62] + acadoWorkspace.g[63]*acadoWorkspace.x[63] + acadoWorkspace.g[64]*acadoWorkspace.x[64] + acadoWorkspace.g[65]*acadoWorkspace.x[65] + acadoWorkspace.g[66]*acadoWorkspace.x[66] + acadoWorkspace.g[67]*acadoWorkspace.x[67];
-kkt = fabs( kkt );
-for (index = 0; index < 68; ++index)
-{
-prd = acadoWorkspace.y[index];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lb[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ub[index] * prd);
-}
-for (index = 0; index < 96; ++index)
-{
-prd = acadoWorkspace.y[index + 68];
-if (prd > 1e-12)
-kkt += fabs(acadoWorkspace.lbA[index] * prd);
-else if (prd < -1e-12)
-kkt += fabs(acadoWorkspace.ubA[index] * prd);
-}
-return kkt;
-}
-
-real_t acado_getObjective(  )
-{
-real_t objVal;
-
-int lRun1;
-/** Row vector of size: 5 */
-real_t tmpDy[ 5 ];
-
-/** Row vector of size: 3 */
-real_t tmpDyN[ 3 ];
-
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 4];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 4 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 4 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 4 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.u[lRun1 * 2];
-acadoWorkspace.objValueIn[5] = acadoVariables.u[lRun1 * 2 + 1];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2 + 1];
-
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.Dy[lRun1 * 5] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 5];
-acadoWorkspace.Dy[lRun1 * 5 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 5 + 1];
-acadoWorkspace.Dy[lRun1 * 5 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 5 + 2];
-acadoWorkspace.Dy[lRun1 * 5 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 5 + 3];
-acadoWorkspace.Dy[lRun1 * 5 + 4] = acadoWorkspace.objValueOut[4] - acadoVariables.y[lRun1 * 5 + 4];
-}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[128];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[129];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[130];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[131];
-acadoWorkspace.objValueIn[4] = acadoVariables.od[64];
-acadoWorkspace.objValueIn[5] = acadoVariables.od[65];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
-acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
-acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
-acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2];
-objVal = 0.0000000000000000e+00;
-for (lRun1 = 0; lRun1 < 32; ++lRun1)
-{
-tmpDy[0] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 5] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 10] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 15] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 20];
-tmpDy[1] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 1] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 6] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 11] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 16] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 21];
-tmpDy[2] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 2] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 7] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 12] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 17] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 22];
-tmpDy[3] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 3] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 8] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 13] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 18] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 23];
-tmpDy[4] = + acadoWorkspace.Dy[lRun1 * 5]*acadoVariables.W[lRun1 * 25 + 4] + acadoWorkspace.Dy[lRun1 * 5 + 1]*acadoVariables.W[lRun1 * 25 + 9] + acadoWorkspace.Dy[lRun1 * 5 + 2]*acadoVariables.W[lRun1 * 25 + 14] + acadoWorkspace.Dy[lRun1 * 5 + 3]*acadoVariables.W[lRun1 * 25 + 19] + acadoWorkspace.Dy[lRun1 * 5 + 4]*acadoVariables.W[lRun1 * 25 + 24];
-objVal += + acadoWorkspace.Dy[lRun1 * 5]*tmpDy[0] + acadoWorkspace.Dy[lRun1 * 5 + 1]*tmpDy[1] + acadoWorkspace.Dy[lRun1 * 5 + 2]*tmpDy[2] + acadoWorkspace.Dy[lRun1 * 5 + 3]*tmpDy[3] + acadoWorkspace.Dy[lRun1 * 5 + 4]*tmpDy[4];
-}
-
-tmpDyN[0] = + acadoWorkspace.DyN[0]*acadoVariables.WN[0] + acadoWorkspace.DyN[1]*acadoVariables.WN[3] + acadoWorkspace.DyN[2]*acadoVariables.WN[6];
-tmpDyN[1] = + acadoWorkspace.DyN[0]*acadoVariables.WN[1] + acadoWorkspace.DyN[1]*acadoVariables.WN[4] + acadoWorkspace.DyN[2]*acadoVariables.WN[7];
-tmpDyN[2] = + acadoWorkspace.DyN[0]*acadoVariables.WN[2] + acadoWorkspace.DyN[1]*acadoVariables.WN[5] + acadoWorkspace.DyN[2]*acadoVariables.WN[8];
-objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + acadoWorkspace.DyN[2]*tmpDyN[2];
-
-objVal *= 0.5;
-return objVal;
-}
-
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py
deleted file mode 100644
index 3b15b08fe..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/libmpc_py.py
+++ /dev/null
@@ -1,34 +0,0 @@
-import os
-
-from cffi import FFI
-from common.ffi_wrapper import suffix
-
-mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__)))
-libmpc_fn = os.path.join(mpc_dir, "libmpc"+suffix())
-
-ffi = FFI()
-ffi.cdef("""
-const int MPC_N = 32;
-
-typedef struct {
-double x_ego, v_ego, a_ego;
-} state_t;
-
-
-typedef struct {
-double x_ego[MPC_N+1];
-double v_ego[MPC_N+1];
-double a_ego[MPC_N+1];
-double t[MPC_N+1];
-double j_ego[MPC_N];
-double cost;
-} log_t;
-
-
-void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost);
-int run_mpc(state_t * x0, log_t * solution,
-            double target_x[MPC_N+1], double target_v[MPC_N+1], double target_a[MPC_N+1],
-            double min_a, double max_a);
-""")
-
-libmpc = ffi.dlopen(libmpc_fn)
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
new file mode 100644
index 000000000..5c8a98bda
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -0,0 +1,362 @@
+#!/usr/bin/env python3
+import os
+import numpy as np
+
+from common.realtime import sec_since_boot
+from common.numpy_fast import clip, interp
+from selfdrive.swaglog import cloudlog
+from selfdrive.modeld.constants import index_function
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+
+from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
+from casadi import SX, vertcat
+
+LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
+EXPORT_DIR = os.path.join(LONG_MPC_DIR, "c_generated_code")
+JSON_FILE = "acados_ocp_long.json"
+
+SOURCES = ['lead0', 'lead1', 'cruise']
+
+X_DIM = 3
+U_DIM = 1
+COST_E_DIM = 4
+COST_DIM = COST_E_DIM + 1
+CONSTR_DIM = 4
+
+X_EGO_OBSTACLE_COST = 3.
+V_EGO_COST = 0.
+X_EGO_COST = 0.
+A_EGO_COST = 0.
+J_EGO_COST = 10.
+DANGER_ZONE_COST = 100.
+CRASH_DISTANCE = .5
+LIMIT_COST = 1e6
+
+
+# Less timestamps doesn't hurt performance and leads to
+# much better convergence of the MPC with low iterations
+N = 12
+MAX_T = 10.0
+T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N+1)]
+
+T_IDXS = np.array(T_IDXS_LST)
+T_DIFFS = np.diff(T_IDXS, prepend=[0.])
+MIN_ACCEL = -3.5
+T_REACT = 1.8
+MAX_BRAKE = 9.81
+
+
+def get_stopped_equivalence_factor(v_lead):
+  return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
+
+def get_safe_obstacle_distance(v_ego):
+  return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
+
+def desired_follow_distance(v_ego, v_lead):
+  return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
+
+
+def gen_long_model():
+  model = AcadosModel()
+  model.name = 'long'
+
+  # set up states & controls
+  x_ego = SX.sym('x_ego')
+  v_ego = SX.sym('v_ego')
+  a_ego = SX.sym('a_ego')
+  model.x = vertcat(x_ego, v_ego, a_ego)
+
+  # controls
+  j_ego = SX.sym('j_ego')
+  model.u = vertcat(j_ego)
+
+  # xdot
+  x_ego_dot = SX.sym('x_ego_dot')
+  v_ego_dot = SX.sym('v_ego_dot')
+  a_ego_dot = SX.sym('a_ego_dot')
+  model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot)
+
+  # live parameters
+  x_obstacle = SX.sym('x_obstacle')
+  a_min = SX.sym('a_min')
+  a_max = SX.sym('a_max')
+  model.p = vertcat(a_min, a_max, x_obstacle)
+
+  # dynamics model
+  f_expl = vertcat(v_ego, a_ego, j_ego)
+  model.f_impl_expr = model.xdot - f_expl
+  model.f_expl_expr = f_expl
+  return model
+
+
+def gen_long_mpc_solver():
+  ocp = AcadosOcp()
+  ocp.model = gen_long_model()
+
+  Tf = T_IDXS[-1]
+
+  # set dimensions
+  ocp.dims.N = N
+
+  # set cost module
+  ocp.cost.cost_type = 'NONLINEAR_LS'
+  ocp.cost.cost_type_e = 'NONLINEAR_LS'
+
+  QR = np.zeros((COST_DIM, COST_DIM))
+  Q = np.zeros((COST_E_DIM, COST_E_DIM))
+
+  ocp.cost.W = QR
+  ocp.cost.W_e = Q
+
+  x_ego, v_ego, a_ego = ocp.model.x[0], ocp.model.x[1], ocp.model.x[2]
+  j_ego = ocp.model.u[0]
+
+  a_min, a_max = ocp.model.p[0], ocp.model.p[1]
+  x_obstacle = ocp.model.p[2]
+
+  ocp.cost.yref = np.zeros((COST_DIM, ))
+  ocp.cost.yref_e = np.zeros((COST_E_DIM, ))
+
+  desired_dist_comfort = get_safe_obstacle_distance(v_ego)
+
+  # The main cost in normal operation is how close you are to the "desired" distance
+  # from an obstacle at every timestep. This obstacle can be a lead car
+  # or other object. In e2e mode we can use x_position targets as a cost
+  # instead.
+  costs = [((x_obstacle - x_ego) - (desired_dist_comfort)) / (v_ego + 10.),
+           x_ego,
+           v_ego,
+           a_ego,
+           j_ego]
+  ocp.model.cost_y_expr = vertcat(*costs)
+  ocp.model.cost_y_expr_e = vertcat(*costs[:-1])
+
+  # Constraints on speed, acceleration and desired distance to
+  # the obstacle, which is treated as a slack constraint so it
+  # behaves like an assymetrical cost.
+  constraints = vertcat((v_ego),
+                        (a_ego - a_min),
+                        (a_max - a_ego),
+                        ((x_obstacle - x_ego) - (3/4) * (desired_dist_comfort)) / (v_ego + 10.))
+  ocp.model.con_h_expr = constraints
+  ocp.model.con_h_expr_e = vertcat(np.zeros(CONSTR_DIM))
+
+  x0 = np.zeros(X_DIM)
+  ocp.constraints.x0 = x0
+  ocp.parameter_values = np.array([-1.2, 1.2, 0.0])
+
+  # We put all constraint cost weights to 0 and only set them at runtime
+  cost_weights = np.zeros(CONSTR_DIM)
+  ocp.cost.zl = cost_weights
+  ocp.cost.Zl = cost_weights
+  ocp.cost.Zu = cost_weights
+  ocp.cost.zu = cost_weights
+
+  ocp.constraints.lh = np.zeros(CONSTR_DIM)
+  ocp.constraints.lh_e = np.zeros(CONSTR_DIM)
+  ocp.constraints.uh = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.uh_e = 1e4*np.ones(CONSTR_DIM)
+  ocp.constraints.idxsh = np.arange(CONSTR_DIM)
+
+  # The HPIPM solver can give decent solutions even when it is stopped early
+  # Which is critical for our purpose where the compute time is strictly bounded
+  # We use HPIPM in the SPEED_ABS mode, which ensures fastest runtime. This
+  # does not cause issues since the problem is well bounded.
+  ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
+  ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
+  ocp.solver_options.integrator_type = 'ERK'
+  ocp.solver_options.nlp_solver_type = 'SQP_RTI'
+
+  # More iterations take too much time and less lead to inaccurate convergence in
+  # some situations. Ideally we would run just 1 iteration to ensure fixed runtime.
+  ocp.solver_options.qp_solver_iter_max = 10
+
+  # set prediction horizon
+  ocp.solver_options.tf = Tf
+  ocp.solver_options.shooting_nodes = T_IDXS
+
+  ocp.code_export_directory = EXPORT_DIR
+  return ocp
+
+
+class LongitudinalMpc():
+  def __init__(self, e2e=False):
+    self.e2e = e2e
+    self.reset()
+    self.accel_limit_arr = np.zeros((N+1, 2))
+    self.accel_limit_arr[:,0] = -1.2
+    self.accel_limit_arr[:,1] = 1.2
+    self.source = SOURCES[2]
+
+  def reset(self):
+    self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
+    self.v_solution = [0.0 for i in range(N+1)]
+    self.a_solution = [0.0 for i in range(N+1)]
+    self.j_solution = [0.0 for i in range(N)]
+    self.yref = np.zeros((N+1, COST_DIM))
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+    self.x_sol = np.zeros((N+1, X_DIM))
+    self.u_sol = np.zeros((N,1))
+    self.params = np.zeros((N+1,3))
+    for i in range(N+1):
+      self.solver.set(i, 'x', np.zeros(X_DIM))
+    self.last_cloudlog_t = 0
+    self.status = False
+    self.crash_cnt = 0.0
+    self.solution_status = 0
+    self.x0 = np.zeros(X_DIM)
+    self.set_weights()
+
+  def set_weights(self):
+    if self.e2e:
+      self.set_weights_for_xva_policy()
+    else:
+      self.set_weights_for_lead_policy()
+
+  def set_weights_for_lead_policy(self):
+    W = np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])
+    Ws = np.tile(W[None], reps=(N,1,1))
+    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
+    self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
+
+    # Set L2 slack cost on lower bound constraints
+    Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
+    Zls = np.tile(Zl[None], reps=(N+1,1,1))
+    self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
+
+  def set_weights_for_xva_policy(self):
+    W = np.diag([0., 10., 1., 10., 1.])
+    Ws = np.tile(W[None], reps=(N,1,1))
+    self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
+    # Setting the slice without the copy make the array not contiguous,
+    # causing issues with the C interface.
+    self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
+
+    # Set L2 slack cost on lower bound constraints
+    Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
+    Zls = np.tile(Zl[None], reps=(N+1,1,1))
+    self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
+
+  def set_cur_state(self, v, a):
+    if abs(self.x0[1] - v) > 1.:
+      self.x0[1] = v
+      self.x0[2] = a
+      for i in range(0, N+1):
+        self.solver.set(i, 'x', self.x0)
+    else:
+      self.x0[1] = v
+      self.x0[2] = a
+
+  def extrapolate_lead(self, x_lead, v_lead, a_lead, a_lead_tau):
+    a_lead_traj = a_lead * np.exp(-a_lead_tau * (T_IDXS**2)/2.)
+    v_lead_traj = np.clip(v_lead + np.cumsum(T_DIFFS * a_lead_traj), 0.0, 1e8)
+    x_lead_traj = x_lead + np.cumsum(T_DIFFS * v_lead_traj)
+    lead_xv = np.column_stack((x_lead_traj, v_lead_traj))
+    return lead_xv
+
+  def process_lead(self, lead):
+    v_ego = self.x0[1]
+    if lead is not None and lead.status:
+      x_lead = lead.dRel
+      v_lead = lead.vLead
+      a_lead = lead.aLeadK
+      a_lead_tau = lead.aLeadTau
+    else:
+      # Fake a fast lead car, so mpc can keep running in the same mode
+      x_lead = 50.0
+      v_lead = v_ego + 10.0
+      a_lead = 0.0
+      a_lead_tau = _LEAD_ACCEL_TAU
+
+    # MPC will not converge if immediate crash is expected
+    # Clip lead distance to what is still possible to brake for
+    min_x_lead = ((v_ego + v_lead)/2) * (v_ego - v_lead) / (-MIN_ACCEL * 2)
+    x_lead = clip(x_lead, min_x_lead, 1e8)
+    v_lead = clip(v_lead, 0.0, 1e8)
+    a_lead = clip(a_lead, -10., 5.)
+    lead_xv = self.extrapolate_lead(x_lead, v_lead, a_lead, a_lead_tau)
+    return lead_xv
+
+  def set_accel_limits(self, min_a, max_a):
+    self.cruise_min_a = min_a
+    self.cruise_max_a = max_a
+
+  def update(self, carstate, radarstate, v_cruise):
+    v_ego = self.x0[1]
+    self.status = radarstate.leadOne.status or radarstate.leadTwo.status
+
+    lead_xv_0 = self.process_lead(radarstate.leadOne)
+    lead_xv_1 = self.process_lead(radarstate.leadTwo)
+
+    # set accel limits in params
+    self.params[:,0] = interp(float(self.status), [0.0, 1.0], [self.cruise_min_a, MIN_ACCEL])
+    self.params[:,1] = self.cruise_max_a
+
+    # To estimate a safe distance from a moving lead, we calculate how much stopping
+    # distance that lead needs as a minimum. We can add that to the current distance
+    # and then treat that as a stopped car/obstacle at this new distance.
+    lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])
+    lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1])
+
+    # Fake an obstacle for cruise, this ensures smooth acceleration to set speed
+    # when the leads are no factor.
+    cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS
+    cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS
+    v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
+                               cruise_lower_bound,
+                               cruise_upper_bound)
+    cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped)
+
+    x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
+    self.source = SOURCES[np.argmin(x_obstacles[0])]
+    self.params[:,2] = np.min(x_obstacles, axis=1)
+
+    self.run()
+    if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
+            radarstate.leadOne.modelProb > 0.9):
+      self.crash_cnt += 1
+    else:
+      self.crash_cnt = 0
+
+  def update_with_xva(self, x, v, a):
+    self.yref[:,1] = x
+    self.yref[:,2] = v
+    self.yref[:,3] = a
+    self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old')
+    self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
+    self.accel_limit_arr[:,0] = -10.
+    self.accel_limit_arr[:,1] = 10.
+    x_obstacle = 1e5*np.ones((N+1))
+    self.params = np.concatenate([self.accel_limit_arr,
+                             x_obstacle[:,None]], axis=1)
+    self.run()
+
+
+  def run(self):
+    for i in range(N+1):
+      self.solver.set_param(i, self.params[i])
+    self.solver.constraints_set(0, "lbx", self.x0)
+    self.solver.constraints_set(0, "ubx", self.x0)
+    self.solution_status = self.solver.solve()
+    self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
+    self.solver.fill_in_slice(0, N, 'u', self.u_sol)
+
+    self.v_solution = self.x_sol[:,1]
+    self.a_solution = self.x_sol[:,2]
+    self.j_solution = self.u_sol[:,0]
+
+    t = sec_since_boot()
+    if self.solution_status != 0:
+      if t > self.last_cloudlog_t + 5.0:
+        self.last_cloudlog_t = t
+        cloudlog.warning("Long mpc reset, solution_status: %s" % (
+                          self.solution_status))
+      self.reset()
+
+
+if __name__ == "__main__":
+  ocp = gen_long_mpc_solver()
+  AcadosOcpSolver.generate(ocp, json_file=JSON_FILE, build=False)
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c
deleted file mode 100644
index c392609da..000000000
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/longitudinal_mpc.c
+++ /dev/null
@@ -1,108 +0,0 @@
-#include "acado_common.h"
-#include "acado_auxiliary_functions.h"
-#include "common/modeldata.h"
-
-#include <stdio.h>
-#include <math.h>
-
-#define NX          ACADO_NX  /* Number of differential state variables.  */
-#define NXA         ACADO_NXA /* Number of algebraic variables. */
-#define NU          ACADO_NU  /* Number of control inputs. */
-#define NOD         ACADO_NOD  /* Number of online data values. */
-
-#define NY          ACADO_NY  /* Number of measurements/references on nodes 0..N - 1. */
-#define NYN         ACADO_NYN /* Number of measurements/references on node N. */
-
-#define N           ACADO_N   /* Number of intervals in the horizon. */
-
-ACADOvariables acadoVariables;
-ACADOworkspace acadoWorkspace;
-
-typedef struct {
-  double x_ego, v_ego, a_ego;
-} state_t;
-
-
-typedef struct {
-  double x_ego[N+1];
-  double v_ego[N+1];
-  double a_ego[N+1];
-  double t[N+1];
-  double j_ego[N];
-  double cost;
-} log_t;
-
-void init(double xCost, double vCost, double aCost, double jerkCost, double constraintCost){
-  acado_initializeSolver();
-  int    i;
-  const int STEP_MULTIPLIER = 3;
-
-  /* Initialize the states and controls. */
-  for (i = 0; i < NX * (N + 1); ++i)  acadoVariables.x[ i ] = 0.0;
-  for (i = 0; i < NU * N; ++i)  acadoVariables.u[ i ] = 0.0;
-
-  /* Initialize the measurements/reference. */
-  for (i = 0; i < NY * N; ++i)  acadoVariables.y[ i ] = 0.0;
-  for (i = 0; i < NYN; ++i)  acadoVariables.yN[ i ] = 0.0;
-
-  /* MPC: initialize the current state feedback. */
-  for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0;
-  
-  // Set weights
-  for (i = 0; i < N; i++) {
-    double f = 20 * (T_IDXS[i+1] - T_IDXS[i]);
-    // Setup diagonal entries
-    acadoVariables.W[NY*NY*i + (NY+1)*0] = xCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*1] = vCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*2] = aCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*3] = jerkCost * f;
-    acadoVariables.W[NY*NY*i + (NY+1)*4] = constraintCost * f;
-  }
-  acadoVariables.WN[(NYN+1)*0] = xCost * STEP_MULTIPLIER;
-  acadoVariables.WN[(NYN+1)*1] = vCost * STEP_MULTIPLIER;
-  acadoVariables.WN[(NYN+1)*2] = aCost * STEP_MULTIPLIER;
-
-}
-
-
-int run_mpc(state_t * x0, log_t * solution,
-            double target_x[N+1], double target_v[N+1], double target_a[N+1],
-            double min_a, double max_a){
-  int i;
-  for (i = 0; i < N + 1; ++i){
-    acadoVariables.od[i*NOD] = min_a;
-    acadoVariables.od[i*NOD+1] = max_a;
-  }
-  for (i = 0; i < N; i+= 1){
-    acadoVariables.y[NY*i + 0] = target_x[i];
-    acadoVariables.y[NY*i + 1] = target_v[i];
-    acadoVariables.y[NY*i + 2] = target_a[i];
-    acadoVariables.y[NY*i + 3] = 0.0;
-    acadoVariables.y[NY*i + 4] = 0.0;
-  }
-  acadoVariables.yN[0] = target_x[N];
-  acadoVariables.yN[1] = target_v[N];
-  acadoVariables.yN[2] = target_a[N];
-
-  acadoVariables.x0[0] = x0->x_ego;
-  acadoVariables.x0[1] = x0->v_ego;
-  acadoVariables.x0[2] = x0->a_ego;
-
-  acado_preparationStep();
-  acado_feedbackStep();
-
-  for (i = 0; i <= N; i++) {
-    solution->x_ego[i] = acadoVariables.x[i*NX];
-    solution->v_ego[i] = acadoVariables.x[i*NX+1];
-    solution->a_ego[i] = acadoVariables.x[i*NX+2];
-
-    if (i < N) {
-      solution->j_ego[i] = acadoVariables.u[i*NU];
-    }
-  }
-  solution->cost = acado_getObjective();
-
-  // Dont shift states here. Current solution is closer to next timestep than if
-  // we shift by 0.1 seconds.
-  return acado_getNWSR();
-}
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index cab3e20aa..051a68a74 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -4,15 +4,12 @@ import numpy as np
 from common.numpy_fast import interp
 
 import cereal.messaging as messaging
-from cereal import log
 from common.realtime import DT_MDL
-from common.realtime import sec_since_boot
 from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
-from selfdrive.controls.lib.fcw import FCWChecker
 from selfdrive.controls.lib.longcontrol import LongCtrlState
-from selfdrive.controls.lib.lead_mpc import LeadMpc
-from selfdrive.controls.lib.long_mpc import LongitudinalMpc
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpc
+from selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import T_IDXS as T_IDXS_MPC
 from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
 from selfdrive.swaglog import cloudlog
 
@@ -45,29 +42,22 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
 
 
 class Planner():
-  def __init__(self, CP):
+  def __init__(self, CP, init_v=0.0, init_a=0.0):
     self.CP = CP
-    self.mpcs = {}
-    self.mpcs['lead0'] = LeadMpc(0)
-    self.mpcs['lead1'] = LeadMpc(1)
-    self.mpcs['cruise'] = LongitudinalMpc()
+    self.mpc = LongitudinalMpc()
 
     self.fcw = False
-    self.fcw_checker = FCWChecker()
 
-    self.v_desired = 0.0
-    self.a_desired = 0.0
-    self.longitudinalPlanSource = 'cruise'
+    self.v_desired = init_v
+    self.a_desired = init_a
     self.alpha = np.exp(-DT_MDL/2.0)
-    self.lead_0 = log.ModelDataV2.LeadDataV3.new_message()
-    self.lead_1 = log.ModelDataV2.LeadDataV3.new_message()
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
+    self.j_desired_trajectory = np.zeros(CONTROL_N)
 
 
   def update(self, sm, CP):
-    cur_time = sec_since_boot()
     v_ego = sm['carState'].vEgo
     a_ego = sm['carState'].aEgo
 
@@ -78,9 +68,6 @@ class Planner():
     long_control_state = sm['controlsState'].longControlState
     force_slow_decel = sm['controlsState'].forceDecel
 
-    self.lead_0 = sm['radarState'].leadOne
-    self.lead_1 = sm['radarState'].leadTwo
-
     enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
     if not enabled or sm['carState'].gasPressed:
       self.v_desired = v_ego
@@ -97,33 +84,19 @@ class Planner():
       accel_limits_turns[1] = min(accel_limits_turns[1], AWARENESS_DECEL)
       accel_limits_turns[0] = min(accel_limits_turns[0], accel_limits_turns[1])
     # clip limits, cannot init MPC outside of bounds
-    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired)
-    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired)
-    self.mpcs['cruise'].set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+    accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
+    accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
+    self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
+    self.mpc.set_cur_state(self.v_desired, self.a_desired)
+    self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+    self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
+    self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
+    self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
 
-    next_a = np.inf
-    for key in self.mpcs:
-      self.mpcs[key].set_cur_state(self.v_desired, self.a_desired)
-      self.mpcs[key].update(sm['carState'], sm['radarState'], v_cruise)
-      if self.mpcs[key].status and self.mpcs[key].a_solution[5] < next_a:  # picks slowest solution from accel in ~0.2 seconds
-        self.longitudinalPlanSource = key
-        self.v_desired_trajectory = self.mpcs[key].v_solution[:CONTROL_N]
-        self.a_desired_trajectory = self.mpcs[key].a_solution[:CONTROL_N]
-        self.j_desired_trajectory = self.mpcs[key].j_solution[:CONTROL_N]
-        next_a = self.mpcs[key].a_solution[5]
-
-    # determine fcw
-    if self.mpcs['lead0'].new_lead:
-      self.fcw_checker.reset_lead(cur_time)
-    blinkers = sm['carState'].leftBlinker or sm['carState'].rightBlinker
-    self.fcw = self.fcw_checker.update(self.mpcs['lead0'].mpc_solution, cur_time,
-                                       sm['controlsState'].active,
-                                       v_ego, sm['carState'].aEgo,
-                                       self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
-                                       self.lead_1.yRel, self.lead_1.vLat,
-                                       self.lead_1.fcw, blinkers) and not sm['carState'].brakePressed
+    #TODO counter is only needed because radar is glitchy, remove once radar is gone
+    self.fcw = self.mpc.crash_cnt > 5
     if self.fcw:
-      cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
+      cloudlog.info("FCW triggered")
 
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
@@ -143,8 +116,8 @@ class Planner():
     longitudinalPlan.accels = [float(x) for x in self.a_desired_trajectory]
     longitudinalPlan.jerks = [float(x) for x in self.j_desired_trajectory]
 
-    longitudinalPlan.hasLead = self.mpcs['lead0'].status
-    longitudinalPlan.longitudinalPlanSource = self.longitudinalPlanSource
+    longitudinalPlan.hasLead = sm['radarState'].leadOne.status
+    longitudinalPlan.longitudinalPlanSource = self.mpc.source
     longitudinalPlan.fcw = self.fcw
 
     pm.send('longitudinalPlan', plan_send)
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 235bafc41..d75048826 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -30,7 +30,7 @@ def plannerd_thread(sm=None, pm=None):
                              poll=['radarState', 'modelV2'], ignore_avg_freq=['radarState'])
 
   if pm is None:
-    pm = messaging.PubMaster(['longitudinalPlan', 'liveLongitudinalMpc', 'lateralPlan', 'liveMpc'])
+    pm = messaging.PubMaster(['longitudinalPlan', 'lateralPlan'])
 
   while True:
     sm.update()
diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py
index 7ea26d62f..cb6ff2900 100755
--- a/selfdrive/debug/can_printer.py
+++ b/selfdrive/debug/can_printer.py
@@ -1,42 +1,43 @@
 #!/usr/bin/env python3
+import argparse
 import binascii
-import os
-import sys
 from collections import defaultdict
 
 import cereal.messaging as messaging
 from common.realtime import sec_since_boot
 
 
-def can_printer(bus=0, max_msg=None, addr="127.0.0.1"):
+def can_printer(bus, max_msg, addr):
   logcan = messaging.sub_sock('can', addr=addr)
 
   start = sec_since_boot()
   lp = sec_since_boot()
   msgs = defaultdict(list)
-  canbus = int(os.getenv("CAN", bus))
   while 1:
     can_recv = messaging.drain_sock(logcan, wait_for_one=True)
     for x in can_recv:
       for y in x.can:
-        if y.src == canbus:
+        if y.src == bus:
           msgs[y.address].append(y.dat)
 
     if sec_since_boot() - lp > 0.1:
       dd = chr(27) + "[2J"
       dd += "%5.2f\n" % (sec_since_boot() - start)
-      for k, v in sorted(zip(msgs.keys(), map(lambda x: binascii.hexlify(x[-1]), msgs.values()))):
-        if max_msg is None or k < max_msg:
-          dd += "%s(%6d) %s\n" % ("%04X(%4d)" % (k, k), len(msgs[k]), v.decode('ascii'))
+      for addr in sorted(msgs.keys()):
+        a = msgs[addr][-1].decode('ascii', 'backslashreplace')
+        x = binascii.hexlify(msgs[addr][-1]).decode('ascii')
+        if max_msg is None or addr < max_msg:
+          dd += "%04X(%4d)(%6d) %s \"%s\"\n" % (addr, addr, len(msgs[addr]), x.ljust(20), a)
       print(dd)
       lp = sec_since_boot()
 
 if __name__ == "__main__":
-  if len(sys.argv) > 3:
-    can_printer(int(sys.argv[1]), int(sys.argv[2]), sys.argv[3])
-  elif len(sys.argv) > 2:
-    can_printer(int(sys.argv[1]), int(sys.argv[2]))
-  elif len(sys.argv) > 1:
-    can_printer(int(sys.argv[1]))
-  else:
-    can_printer()
+  parser = argparse.ArgumentParser(description="simple CAN data viewer",
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+
+  parser.add_argument("--bus", type=int, help="CAN bus to print out", default=0)
+  parser.add_argument("--max_msg", type=int, help="max addr ")
+  parser.add_argument("--addr", default="127.0.0.1")
+
+  args = parser.parse_args()
+  can_printer(args.bus, args.max_msg, args.addr)
diff --git a/selfdrive/debug/check_timings.py b/selfdrive/debug/check_timings.py
index 000a73b78..03e39fd70 100755
--- a/selfdrive/debug/check_timings.py
+++ b/selfdrive/debug/check_timings.py
@@ -18,7 +18,7 @@ if __name__ == "__main__":
       for m in msgs:
         ts[s].append(m.logMonoTime / 1e6)
 
-      if len(ts[s]) == ts[s].maxlen:
+      if len(ts[s]):
         d = np.diff(ts[s])
-        print(f"{s:17} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}")
+        print(f"{s:25} {np.mean(d):.2f} {np.std(d):.2f} {np.max(d):.2f} {np.min(d):.2f}")
     time.sleep(1)
diff --git a/selfdrive/debug/count_events.py b/selfdrive/debug/count_events.py
index da207b42b..8b32ce9d2 100755
--- a/selfdrive/debug/count_events.py
+++ b/selfdrive/debug/count_events.py
@@ -14,6 +14,8 @@ if __name__ == "__main__":
   cnt_events: Counter = Counter()
 
   for q in tqdm(r.qlog_paths()):
+    if q is None:
+      continue
     lr = list(LogReader(q))
     for msg in lr:
       if msg.which() == 'carEvents':
diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py
index c35791682..47a37933c 100755
--- a/selfdrive/debug/cycle_alerts.py
+++ b/selfdrive/debug/cycle_alerts.py
@@ -21,10 +21,10 @@ def cycle_alerts(duration=2000, is_metric=False):
   alerts = [EventName.preLaneChangeLeft, EventName.preLaneChangeRight]
 
   CP = CarInterface.get_params("HONDA CIVIC 2016")
-  sm = messaging.SubMaster(['deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration',
+  sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
                             'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman'])
 
-  pm = messaging.PubMaster(['controlsState', 'pandaState', 'deviceState'])
+  pm = messaging.PubMaster(['controlsState', 'pandaStates', 'deviceState'])
 
   events = Events()
   AM = AlertManager()
@@ -61,10 +61,9 @@ def cycle_alerts(duration=2000, is_metric=False):
     dat.deviceState.started = True
     pm.send('deviceState', dat)
 
-    dat = messaging.new_message()
-    dat.init('pandaState')
-    dat.pandaState.ignitionLine = True
-    pm.send('pandaState', dat)
+    dat = messaging.new_message('pandaStates', 1)
+    dat.pandaStates[0].ignitionLine = True
+    pm.send('pandaStates', dat)
 
     time.sleep(0.01)
 
diff --git a/selfdrive/debug/disable_ecu.py b/selfdrive/debug/disable_ecu.py
index 9dace9a68..f0faf4001 100644
--- a/selfdrive/debug/disable_ecu.py
+++ b/selfdrive/debug/disable_ecu.py
@@ -10,7 +10,7 @@ EXT_DIAG_RESPONSE = b'\x50\x03'
 COM_CONT_REQUEST = b'\x28\x83\x03'
 COM_CONT_RESPONSE = b''
 
-def disable_ecu(ecu_addr, logcan, sendcan, bus, timeout=0.1, retry=5, debug=False):
+def disable_ecu(ecu_addr, logcan, sendcan, bus, timeout=0.5, retry=5, debug=False):
   print(f"ecu disable {hex(ecu_addr)} ...")
   for i in range(retry):
     try:
diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py
index ce96b360f..047c874fb 100755
--- a/selfdrive/debug/dump.py
+++ b/selfdrive/debug/dump.py
@@ -13,7 +13,7 @@ from cereal.services import service_list
 
 if __name__ == "__main__":
 
-  parser = argparse.ArgumentParser(description='Sniff a communcation socket')
+  parser = argparse.ArgumentParser(description='Dump communcation sockets. See cereal/services.py for a complete list of available sockets.')
   parser.add_argument('--pipe', action='store_true')
   parser.add_argument('--raw', action='store_true')
   parser.add_argument('--json', action='store_true')
@@ -21,7 +21,7 @@ if __name__ == "__main__":
   parser.add_argument('--no-print', action='store_true')
   parser.add_argument('--addr', default='127.0.0.1')
   parser.add_argument('--values', help='values to monitor (instead of entire event)')
-  parser.add_argument("socket", type=str, nargs='*', help="socket name")
+  parser.add_argument("socket", type=str, nargs='*', help="socket names to dump. defaults to all services defined in cereal")
   args = parser.parse_args()
 
   if args.addr != "127.0.0.1":
diff --git a/selfdrive/debug/hyundai_enable_radar_points.py b/selfdrive/debug/hyundai_enable_radar_points.py
new file mode 100755
index 000000000..ce03ba03f
--- /dev/null
+++ b/selfdrive/debug/hyundai_enable_radar_points.py
@@ -0,0 +1,104 @@
+#!/usr/bin/env python3
+"""Some Hyundai radars can be reconfigured to output (debug) radar points on bus 1.
+Reconfiguration is done over UDS by reading/writing to 0x0142 using the Read/Write Data By Identifier
+endpoints (0x22 & 0x2E). This script checks your radar firmware version against a list of known 
+firmware versions. If you want to try on a new radar make sure to note the default config value
+in case it's different from the other radars and you need to revert the changes.
+
+After changing the config the car should not show any faults when openpilot is not running. 
+These config changes are persistent accross car reboots. You need to run this script again
+to go back to the default values.
+
+USE AT YOUR OWN RISK! Safety features, like AEB and FCW, might be affected by these changes."""
+
+import sys
+import argparse
+from subprocess import check_output, CalledProcessError
+
+from panda.python import Panda
+from panda.python.uds import UdsClient, SESSION_TYPE, DATA_IDENTIFIER_TYPE
+
+# If your radar supports changing data identifier 0x0142 as well make a PR to
+# this file to add your firmware version. Make sure to post a drive as proof!
+# NOTE: these firmware versions do not match what openpilot uses
+#       because this script uses a different diagnostic session type
+SUPPORTED_FW_VERSIONS = {
+  # 2020 SONATA
+  b"DN8_ SCC FHCUP      1.00 1.00 99110-L0000\x19\x08)\x15T    ": {
+    "default_config": b"\x00\x00\x00\x01\x00\x00",
+    "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  },
+  # 2021 SONATA HYBRID
+  b"DNhe SCC FHCUP      1.00 1.02 99110-L5000 \x01#\x15#    ": {
+    "default_config": b"\x00\x00\x00\x01\x00\x00",
+    "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  }, 
+  # 2020 PALISADE
+  b"LX2_ SCC FHCUP      1.00 1.04 99110-S8100\x19\x05\x02\x16V    ": {
+    "default_config": b"\x00\x00\x00\x01\x00\x00",
+    "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  },
+  # 2020 SANTA FE
+  b"TM__ SCC F-CUP      1.00 1.03 99110-S2000\x19\x050\x13'    ": {
+    "default config": b"\x00\x00\x00\x01\x00\x00",
+    "tracks_enabled": b"\x00\x00\x00\x01\x00\x01",
+  }
+}
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(description='configure radar to output points (or reset to default)')
+  parser.add_argument('--default', action="store_true", default=False, help='reset to default configuration (default: false)')
+  parser.add_argument('--debug', action="store_true", default=False, help='enable debug output (default: false)')
+  parser.add_argument('--bus', type=int, default=0, help='can bus to use (default: 0)')
+  args = parser.parse_args()
+
+  try:
+    check_output(["pidof", "boardd"])
+    print("boardd is running, please kill openpilot before running this script! (aborted)")
+    sys.exit(1)
+  except CalledProcessError as e:
+    if e.returncode != 1: # 1 == no process found (boardd not running)
+      raise e
+
+  confirm = input("power on the vehicle keeping the engine off (press start button twice) then type OK to continue: ").upper().strip()
+  if confirm != "OK":
+    print("\nyou didn't type 'OK! (aborted)")
+    sys.exit(0)
+
+  panda = Panda() # type: ignore
+  panda.set_safety_mode(Panda.SAFETY_ELM327)
+  uds_client = UdsClient(panda, 0x7D0, bus=args.bus, debug=args.debug)
+
+  print("\n[START DIAGNOSTIC SESSION]")
+  session_type : SESSION_TYPE = 0x07 # type: ignore
+  uds_client.diagnostic_session_control(session_type)
+
+  print("[HARDWARE/SOFTWARE VERSION]")
+  fw_version_data_id : DATA_IDENTIFIER_TYPE = 0xf100 # type: ignore
+  fw_version = uds_client.read_data_by_identifier(fw_version_data_id)
+  print(fw_version)
+  if fw_version not in SUPPORTED_FW_VERSIONS.keys():
+    print("radar not supported! (aborted)")
+    sys.exit(1)
+
+  print("[GET CONFIGURATION]")
+  config_data_id : DATA_IDENTIFIER_TYPE = 0x0142 # type: ignore
+  current_config = uds_client.read_data_by_identifier(config_data_id)
+  new_config = SUPPORTED_FW_VERSIONS[fw_version]["default_config" if args.default else "tracks_enabled"]
+  print(f"current config: 0x{current_config.hex()}")
+  if current_config != new_config:
+    print("[CHANGE CONFIGURATION]")
+    print(f"new config:     0x{new_config.hex()}")
+    uds_client.write_data_by_identifier(config_data_id, new_config)
+    if not args.default and current_config != SUPPORTED_FW_VERSIONS[fw_version]["default_config"]:
+      print("\ncurrent config does not match expected default! (aborted)")
+      sys.exit(1)
+
+    print("[DONE]")
+    print("\nrestart your vehicle and ensure there are no faults")
+    if not args.default:
+      print("you can run this script again with --default to go back to the original (factory) settings")
+  else:
+    print("[DONE]")
+    print("\ncurrent config is already the desired configuration")
+    sys.exit(0)
diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py
index de854f070..5589b0d6d 100755
--- a/selfdrive/debug/live_cpu_and_temp.py
+++ b/selfdrive/debug/live_cpu_and_temp.py
@@ -37,8 +37,8 @@ if __name__ == "__main__":
 
   last_temp = 0.0
   last_mem = 0.0
-  total_times = [0., 0., 0., 0.]
-  busy_times = [0., 0., 0.0, 0.]
+  total_times = [0.]*8
+  busy_times = [0.]*8
 
   prev_proclog = None
   prev_proclog_t = None
@@ -54,16 +54,16 @@ if __name__ == "__main__":
     if sm.updated['procLog']:
       m = sm['procLog']
 
-      cores = [0., 0., 0., 0.]
-      total_times_new = [0., 0., 0., 0.]
-      busy_times_new = [0., 0., 0.0, 0.]
+      cores = [0.]*8
+      total_times_new = [0.]*8
+      busy_times_new = [0.]*8
 
       for c in m.cpuTimes:
         n = c.cpuNum
         total_times_new[n] = cputime_total(c)
         busy_times_new[n] = cputime_busy(c)
 
-      for n in range(4):
+      for n in range(8):
         t_busy = busy_times_new[n] - busy_times[n]
         t_total = total_times_new[n] - total_times[n]
         cores[n] = t_busy / t_total
@@ -71,7 +71,7 @@ if __name__ == "__main__":
       total_times = total_times_new[:]
       busy_times = busy_times_new[:]
 
-      print("CPU %.2f%% - RAM: %.2f - Temp %.2f" % (100. * mean(cores), last_mem, last_temp))
+      print("CPU %.2f%% - RAM: %.2f%% - Temp %.2fC" % (100. * mean(cores), last_mem, last_temp))
 
       if args.cpu and prev_proclog is not None:
         procs = {}
diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py
index 6a49a84d8..44b3ba16a 100755
--- a/selfdrive/debug/test_fw_query_on_routes.py
+++ b/selfdrive/debug/test_fw_query_on_routes.py
@@ -71,8 +71,8 @@ if __name__ == "__main__":
       dongles.append(dongle_id)
 
       for msg in lr:
-        if msg.which() == "pandaState":
-          if msg.pandaState.pandaType not in ['uno', 'blackPanda', 'dos']:
+        if msg.which() == "pandaStates":
+          if msg.pandaStates[0].pandaType not in ['uno', 'blackPanda', 'dos']:
             break
 
         elif msg.which() == "carParams":
diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py
index 18c76c3e3..319ead24e 100755
--- a/selfdrive/debug/uiview.py
+++ b/selfdrive/debug/uiview.py
@@ -4,17 +4,19 @@ import cereal.messaging as messaging
 from selfdrive.manager.process_config import managed_processes
 
 if __name__ == "__main__":
-  services = ['controlsState', 'deviceState', 'pandaState']  # the services needed to be spoofed to start ui offroad
   procs = ['camerad', 'ui', 'modeld', 'calibrationd']
 
   for p in procs:
     managed_processes[p].start()
 
-  pm = messaging.PubMaster(services)
+  pm = messaging.PubMaster(['controlsState', 'deviceState', 'pandaStates', 'carParams'])
 
-  msgs = {s: messaging.new_message(s) for s in services}
+  msgs = {s: messaging.new_message(s) for s in ['controlsState', 'deviceState', 'carParams']}
   msgs['deviceState'].deviceState.started = True
-  msgs['pandaState'].pandaState.ignitionLine = True
+  msgs['carParams'].carParams.openpilotLongitudinalControl = True
+
+  msgs['pandaStates'] = messaging.new_message('pandaStates', 1)
+  msgs['pandaStates'].pandaStates[0].ignitionLine = True
 
   try:
     while True:
diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py
index 0a1976d0c..a1b67cb38 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -110,6 +110,10 @@ class HardwareBase:
   def set_screen_brightness(self, percentage):
     pass
 
+  @abstractmethod
+  def get_screen_brightness(self):
+    pass
+
   @abstractmethod
   def set_power_save(self, powersave_enabled):
     pass
@@ -122,6 +126,14 @@ class HardwareBase:
   def get_modem_version(self):
     pass
 
+  @abstractmethod
+  def get_modem_temperatures(self):
+    pass
+
+  @abstractmethod
+  def get_nvme_temperatures(self):
+    pass
+
   @abstractmethod
   def initialize_hardware(self):
     pass
diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py
index b555ae351..2296c26e6 100644
--- a/selfdrive/hardware/eon/hardware.py
+++ b/selfdrive/hardware/eon/hardware.py
@@ -375,6 +375,13 @@ class Android(HardwareBase):
     with open("/sys/class/leds/lcd-backlight/brightness", "w") as f:
       f.write(str(int(percentage * 2.55)))
 
+  def get_screen_brightness(self):
+    try:
+      with open("/sys/class/leds/lcd-backlight/brightness") as f:
+        return int(float(f.read()) / 2.55)
+    except Exception:
+      return 0
+
   def set_power_save(self, powersave_enabled):
     pass
 
@@ -389,6 +396,13 @@ class Android(HardwareBase):
   def get_modem_version(self):
     return None
 
+  def get_modem_temperatures(self):
+    # Not sure if we can get this on the LeEco
+    return []
+
+  def get_nvme_temperatures(self):
+    return []
+
   def initialize_hardware(self):
     pass
 
diff --git a/installer/updater/update.json b/selfdrive/hardware/eon/neos.json
similarity index 100%
rename from installer/updater/update.json
rename to selfdrive/hardware/eon/neos.json
diff --git a/selfdrive/hardware/eon/neos.py b/selfdrive/hardware/eon/neos.py
new file mode 100755
index 000000000..6f290fbcd
--- /dev/null
+++ b/selfdrive/hardware/eon/neos.py
@@ -0,0 +1,130 @@
+#!/usr/bin/env python3
+import argparse
+import hashlib
+import json
+import logging
+import os
+import requests
+
+NEOSUPDATE_DIR = "/data/neoupdate"
+
+RECOVERY_DEV = "/dev/block/bootdevice/by-name/recovery"
+RECOVERY_COMMAND = "/cache/recovery/command"
+
+
+def get_fn(url: str):
+  return os.path.join(NEOSUPDATE_DIR, os.path.basename(url))
+
+
+def download_file(url: str, fn: str, sha256: str, display_name: str, cloudlog=logging) -> None:
+  # check if already downloaded
+  if check_hash(fn, sha256):
+    cloudlog.info(f"{display_name} already cached")
+    return
+
+  try:
+    with open(fn, "ab+") as f:
+      headers = {"Range": f"bytes={f.tell()}-"}
+      r = requests.get(url, stream=True, allow_redirects=True, headers=headers)
+      r.raise_for_status()
+
+      total = int(r.headers['Content-Length'])
+      if 'Content-Range' in r.headers:
+        total = int(r.headers['Content-Range'].split('/')[-1])
+
+      for chunk in r.iter_content(chunk_size=1024 * 1024):
+        f.write(chunk)
+        print(f"Downloading {display_name}: {f.tell() / total * 100}", flush=True)
+  except Exception:
+    cloudlog.error("download error")
+    if os.path.isfile(fn):
+      os.unlink(fn)
+    raise
+
+  if not check_hash(fn, sha256):
+    if os.path.isfile(fn):
+      os.unlink(fn)
+    raise Exception("downloaded update failed hash check")
+
+
+def check_hash(fn: str, sha256: str, length: int = -1) -> bool:
+  if not os.path.exists(fn):
+    return False
+
+  h = hashlib.sha256()
+  with open(fn, "rb") as f:
+    while f.tell() != length:
+      r = min(max(0, length - f.tell()), 1024 * 1024) if length > 0 else 1024 * 1024
+      dat = f.read(r)
+      if not dat:
+        break
+      h.update(dat)
+  return h.hexdigest().lower() == sha256.lower()
+
+
+def flash_update(update_fn: str, out_path: str) -> None:
+  with open(update_fn, "rb") as update, open(out_path, "w+b") as out:
+    while True:
+      dat = update.read(8192)
+      if len(dat) == 0:
+        break
+      out.write(dat)
+
+
+def download_neos_update(manifest_path: str, cloudlog=logging) -> None:
+  with open(manifest_path) as f:
+    m = json.load(f)
+
+  os.makedirs(NEOSUPDATE_DIR, exist_ok=True)
+
+  # handle recovery updates
+  if not check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len']):
+    cloudlog.info("recovery needs update")
+    recovery_fn = os.path.join(NEOSUPDATE_DIR, os.path.basename(m['recovery_url']))
+    download_file(m['recovery_url'], recovery_fn, m['recovery_hash'], "recovery", cloudlog)
+
+    flash_update(recovery_fn, RECOVERY_DEV)
+    assert check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len']), "recovery flash corrupted"
+    cloudlog.info("recovery successfully flashed")
+
+  # download OTA update
+  download_file(m['ota_url'], get_fn(m['ota_url']), m['ota_hash'], "system", cloudlog)
+
+
+def verify_update_ready(manifest_path: str) -> bool:
+  with open(manifest_path) as f:
+    m = json.load(f)
+
+  ota_downloaded = check_hash(get_fn(m['ota_url']), m['ota_hash'])
+  recovery_flashed = check_hash(RECOVERY_DEV, m['recovery_hash'], m['recovery_len'])
+  return ota_downloaded and recovery_flashed
+
+
+def perform_ota_update(manifest_path: str) -> None:
+  with open(manifest_path) as f:
+    m = json.load(f)
+
+  # reboot into recovery
+  ota_fn = get_fn(m['ota_url'])
+  with open(RECOVERY_COMMAND, "w") as rf:
+    rf.write(f"--update_package={ota_fn}\n")
+  os.system("service call power 16 i32 0 s16 recovery i32 1")
+
+
+if __name__ == "__main__":
+  parser = argparse.ArgumentParser(description="NEOS update utility",
+                                   formatter_class=argparse.ArgumentDefaultsHelpFormatter)
+  parser.add_argument("--swap", action="store_true", help="Peform update after downloading")
+  parser.add_argument("--swap-if-ready", action="store_true", help="Perform update if already downloaded")
+  parser.add_argument("manifest", help="Manifest json")
+  args = parser.parse_args()
+
+  logging.basicConfig(level=logging.INFO)
+
+  if args.swap_if_ready:
+    if verify_update_ready(args.manifest):
+      perform_ota_update(args.manifest)
+  else:
+    download_neos_update(args.manifest, logging)
+    if args.swap:
+      perform_ota_update(args.manifest)
diff --git a/selfdrive/hardware/eon/updater b/selfdrive/hardware/eon/updater
new file mode 100755
index 000000000..eaf34e957
Binary files /dev/null and b/selfdrive/hardware/eon/updater differ
diff --git a/selfdrive/hardware/hw.h b/selfdrive/hardware/hw.h
index 400b1816f..d7a6c8d1d 100644
--- a/selfdrive/hardware/hw.h
+++ b/selfdrive/hardware/hw.h
@@ -14,6 +14,7 @@ class HardwarePC : public HardwareNone {
 public:
   static std::string get_os_version() { return "openpilot for PC"; }
   static bool PC() { return true; }
+  static bool TICI() { return util::getenv("TICI", 0) == 1; }
 };
 #define Hardware HardwarePC
 #endif
diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py
index 69bca5b62..b2be6ca91 100644
--- a/selfdrive/hardware/pc/hardware.py
+++ b/selfdrive/hardware/pc/hardware.py
@@ -83,6 +83,9 @@ class Pc(HardwareBase):
   def set_screen_brightness(self, percentage):
     pass
 
+  def get_screen_brightness(self):
+    return 0
+
   def set_power_save(self, powersave_enabled):
     pass
 
@@ -92,6 +95,12 @@ class Pc(HardwareBase):
   def get_modem_version(self):
     return None
 
+  def get_modem_temperatures(self):
+    return []
+
+  def get_nvme_temperatures(self):
+    return []
+
   def initialize_hardware(self):
     pass
 
diff --git a/selfdrive/hardware/tici/agnos.json b/selfdrive/hardware/tici/agnos.json
index f30288a8a..14f5567c7 100644
--- a/selfdrive/hardware/tici/agnos.json
+++ b/selfdrive/hardware/tici/agnos.json
@@ -1,9 +1,9 @@
 [
   {
     "name": "boot",
-    "url": "https://commadist.azureedge.net/agnosupdate/boot-e992b0461fc7705ce54f556247cd721b04d4d28885a461cd536389fd0bcb7e9c.img.xz",
-    "hash": "e992b0461fc7705ce54f556247cd721b04d4d28885a461cd536389fd0bcb7e9c",
-    "hash_raw": "e992b0461fc7705ce54f556247cd721b04d4d28885a461cd536389fd0bcb7e9c",
+    "url": "https://commadist.azureedge.net/agnosupdate/boot-ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c.img.xz",
+    "hash": "ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c",
+    "hash_raw": "ab4b6f64a90617ddbebe250f977616d70a25864f82c9c6ea9d88ebc5fe80e37c",
     "size": 14772224,
     "sparse": false,
     "full_check": true,
@@ -41,9 +41,9 @@
   },
   {
     "name": "system",
-    "url": "https://commadist.azureedge.net/agnosupdate/system-c4e5875c90b2a74e075d555cba9d8b50839fd85d76ac71b89884e5b37312cffc.img.xz",
-    "hash": "93ee8c5eac72289ebae245c1e48707865c5ead7e8ae53f9066bae8a0a58d1a48",
-    "hash_raw": "c4e5875c90b2a74e075d555cba9d8b50839fd85d76ac71b89884e5b37312cffc",
+    "url": "https://commadist.azureedge.net/agnosupdate/system-a778d523851d88a78ad7440ab602a80e09decdd1877f9f31ea36a7d7f15970dd.img.xz",
+    "hash": "540ee7184cc6d8c14f94e652a062027dcc7559e47f4b347b6f8abac570521ec6",
+    "hash_raw": "a778d523851d88a78ad7440ab602a80e09decdd1877f9f31ea36a7d7f15970dd",
     "size": 10737418240,
     "sparse": true,
     "full_check": false,
diff --git a/selfdrive/hardware/tici/agnos.py b/selfdrive/hardware/tici/agnos.py
index d3b2ca96e..d62d4dbc5 100755
--- a/selfdrive/hardware/tici/agnos.py
+++ b/selfdrive/hardware/tici/agnos.py
@@ -73,6 +73,11 @@ def unsparsify(f: StreamingDecompressor) -> Generator[bytes, None, None]:
     else:
       raise Exception("Unhandled sparse chunk type")
 
+# noop wrapper with same API as unsparsify() for non sparse images
+def noop(f: StreamingDecompressor) -> Generator[bytes, None, None]:
+  while not f.eof:
+    yield f.read(1024 * 1024)
+
 
 def get_target_slot_number() -> int:
   current_slot = subprocess.check_output(["abctl", "--boot_slot"], encoding='utf-8').strip()
@@ -141,22 +146,20 @@ def flash_partition(target_slot_number: int, partition: dict, cloudlog):
 
   path = get_partition_path(target_slot_number, partition)
   with open(path, 'wb+') as out:
-    partition_size = partition['size']
-
     # Flash partition
-    if partition['sparse']:
-      raw_hash = hashlib.sha256()
-      for chunk in unsparsify(downloader):
-        raw_hash.update(chunk)
-        out.write(chunk)
-        p = int(out.tell() / partition_size * 100)
-        print(f"Installing {partition['name']}: {p}")
+    last_p = 0
+    raw_hash = hashlib.sha256()
+    f = unsparsify if partition['sparse'] else noop
+    for chunk in f(downloader):
+      raw_hash.update(chunk)
+      out.write(chunk)
+      p = int(out.tell() / partition['size'] * 100)
+      if p != last_p:
+        last_p = p
+        print(f"Installing {partition['name']}: {p}", flush=True)
 
-      if raw_hash.hexdigest().lower() != partition['hash_raw'].lower():
-        raise Exception(f"Unsparse hash mismatch '{raw_hash.hexdigest().lower()}'")
-    else:
-      while not downloader.eof:
-        out.write(downloader.read(1024 * 1024))
+    if raw_hash.hexdigest().lower() != partition['hash_raw'].lower():
+      raise Exception(f"Raw hash mismatch '{raw_hash.hexdigest().lower()}'")
 
     if downloader.sha256.hexdigest().lower() != partition['hash'].lower():
       raise Exception("Uncompressed hash mismatch")
diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py
index a4d527152..4ac69a443 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -1,7 +1,8 @@
+import json
 import os
+import subprocess
 from functools import cached_property
 from enum import IntEnum
-import subprocess
 from pathlib import Path
 
 from cereal import log
@@ -223,6 +224,25 @@ class Tici(HardwareBase):
     except Exception:
       return None
 
+  def get_modem_temperatures(self):
+    modem = self.get_modem()
+    try:
+      command_timeout = 0.2
+      temps = modem.Command("AT+QTEMP", int(command_timeout * 1000), dbus_interface=MM_MODEM, timeout=command_timeout)
+      return list(map(int, temps.split(' ')[1].split(',')))
+    except Exception:
+      return []
+
+  def get_nvme_temperatures(self):
+    ret = []
+    try:
+      out = subprocess.check_output("sudo smartctl -aj /dev/nvme0", shell=True)
+      dat = json.loads(out)
+      ret = list(map(int, dat["nvme_smart_health_information_log"]["temperature_sensors"]))
+    except Exception:
+      pass
+    return ret
+
   # We don't have a battery, so let's use some sane constants
   def get_battery_capacity(self):
     return 100
@@ -263,6 +283,13 @@ class Tici(HardwareBase):
     except Exception:
       pass
 
+  def get_screen_brightness(self):
+    try:
+      with open("/sys/class/backlight/panel0-backlight/brightness") as f:
+        return int(float(f.read()) / 10.23)
+    except Exception:
+      return 0
+
   def set_power_save(self, powersave_enabled):
     # amplifier, 100mW at idle
     self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled)
diff --git a/selfdrive/hardware/tici/updater b/selfdrive/hardware/tici/updater
index e3442d59d..1c5775120 100755
Binary files a/selfdrive/hardware/tici/updater and b/selfdrive/hardware/tici/updater differ
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index e49718af7..8844b37f5 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -187,6 +187,14 @@ VectorXd Localizer::get_position_geodetic() {
   return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
 }
 
+VectorXd Localizer::get_state() {
+  return this->kf->get_x();
+}
+
+VectorXd Localizer::get_stdev() {
+  return this->kf->get_P().diagonal().array().sqrt();
+}
+
 void Localizer::handle_sensors(double current_time, const capnp::List<cereal::SensorEventData, capnp::Kind::STRUCT>::Reader& log) {
   // TODO does not yet account for double sensor readings in the log
   for (int i = 0; i < log.size(); i++) {
@@ -261,9 +269,9 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
 
   VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
   VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
-  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(3.0 * log.getVerticalAccuracy(), 2)).asDiagonal();
-  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy(), 2)).asDiagonal();
-
+  MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(10.0 * log.getAccuracy(),2) + std::pow(10.0 * log.getVerticalAccuracy(),2)).asDiagonal();
+  MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
+  
   this->unix_timestamp_millis = log.getTimestamp();
   double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm();
 
@@ -325,13 +333,13 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
   // Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
   trans_calib_std *= 10.0;
   rot_calib_std *= 10.0;
-  VectorXd rot_device_std = rotate_std(this->device_from_calib, rot_calib_std);
-  VectorXd trans_device_std = rotate_std(this->device_from_calib, trans_calib_std);
-
+  MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
+  MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
+  
   this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
-    { (VectorXd(rot_device.rows() + rot_device_std.rows()) << rot_device, rot_device_std).finished() });
+    { rot_device }, { rot_device_cov });
   this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
-    { (VectorXd(trans_device.rows() + trans_device_std.rows()) << trans_device, trans_device_std).finished() });
+    { trans_device }, { trans_device_cov });
 }
 
 void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index 04284ba3b..60fed112c 100755
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -38,6 +38,8 @@ public:
   void build_live_location(cereal::LiveLocationKalman::Builder& fix);
 
   Eigen::VectorXd get_position_geodetic();
+  Eigen::VectorXd get_state();
+  Eigen::VectorXd get_stdev();
 
   void handle_msg_bytes(const char *data, const size_t size);
   void handle_msg(const cereal::Event::Reader& log);
diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc
index 00080ab83..4bc655821 100755
--- a/selfdrive/locationd/models/live_kf.cc
+++ b/selfdrive/locationd/models/live_kf.cc
@@ -80,54 +80,13 @@ std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
 
 std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, std::vector<VectorXd> meas, std::vector<MatrixXdr> R) {
   std::optional<Estimate> r;
-  switch (kind) {
-  case OBSERVATION_CAMERA_ODO_TRANSLATION:
-    r = this->predict_and_update_odo_trans(meas, t, kind);
-    break;
-  case OBSERVATION_CAMERA_ODO_ROTATION:
-    r = this->predict_and_update_odo_rot(meas, t, kind);
-    break;
-  case OBSERVATION_ODOMETRIC_SPEED:
-    r = this->predict_and_update_odo_speed(meas, t, kind);
-    break;
-  default:
-    if (R.size() == 0) {
-      R = this->get_R(kind, meas.size());
-    }
-    r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
-    break;
+  if (R.size() == 0) {
+    R = this->get_R(kind, meas.size());
   }
+  r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
   return r;
 }
 
-std::optional<Estimate> LiveKalman::predict_and_update_odo_speed(std::vector<VectorXd> speed, double t, int kind) {
-  std::vector<MatrixXdr> R;
-  R.assign(speed.size(), (MatrixXdr(1, 1) << std::pow(0.2, 2)).finished().asDiagonal());
-  return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(speed), get_vec_mapmat(R));
-}
-
-std::optional<Estimate> LiveKalman::predict_and_update_odo_trans(std::vector<VectorXd> trans, double t, int kind) {
-  std::vector<VectorXd> z;
-  std::vector<MatrixXdr> R;
-  for (VectorXd& trns : trans) {
-    assert(trns.size() == 6); // TODO remove
-    z.push_back(trns.head(3));
-    R.push_back(trns.segment<3>(3).array().square().matrix().asDiagonal());
-  }
-  return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R));
-}
-
-std::optional<Estimate> LiveKalman::predict_and_update_odo_rot(std::vector<VectorXd> rot, double t, int kind) {
-  std::vector<VectorXd> z;
-  std::vector<MatrixXdr> R;
-  for (VectorXd& rt : rot) {
-    assert(rt.size() == 6); // TODO remove
-    z.push_back(rt.head(3));
-    R.push_back(rt.segment<3>(3).array().square().matrix().asDiagonal());
-  }
-  return this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(z), get_vec_mapmat(R));
-}
-
 Eigen::VectorXd LiveKalman::get_initial_x() {
   return this->initial_x;
 }
diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py
index 3df1416ae..a7aefb297 100755
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@@ -58,7 +58,7 @@ class LiveKalman():
                              10**2, 10**2, 10**2,
                              10**2, 10**2, 10**2,
                              1**2, 1**2, 1**2,
-                             0.05**2, 0.05**2, 0.05**2,
+                             1**2, 1**2, 1**2,
                              0.02**2,
                              1**2, 1**2, 1**2,
                              (0.01)**2, (0.01)**2, (0.01)**2])
diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py
index 3eaf8cec0..914e0d022 100755
--- a/selfdrive/locationd/paramsd.py
+++ b/selfdrive/locationd/paramsd.py
@@ -123,22 +123,20 @@ def main(sm=None, pm=None):
   # When driving in wet conditions the stiffness can go down, and then be too low on the next drive
   # Without a way to detect this we have to reset the stiffness every drive
   params['stiffnessFactor'] = 1.0
-
   learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
-
   angle_offset_average = params['angleOffsetAverageDeg']
   angle_offset = angle_offset_average
 
   while True:
     sm.update()
-
-    for which, updated in sm.updated.items():
-      if updated:
+    for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]):
+      if sm.updated[which]:
         t = sm.logMonoTime[which] * 1e-9
         learner.handle_log(t, which, sm[which])
 
     if sm.updated['liveLocationKalman']:
       x = learner.kf.x
+      P = np.sqrt(learner.kf.P.diagonal())
       if not all(map(math.isfinite, x)):
         cloudlog.error("NaN in liveParameters estimate. Resetting to default values")
         learner = ParamsLearner(CP, CP.steerRatio, 1.0, 0.0)
@@ -162,6 +160,10 @@ def main(sm=None, pm=None):
         0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
         min_sr <= msg.liveParameters.steerRatio <= max_sr,
       ))
+      msg.liveParameters.steerRatioStd = float(P[States.STEER_RATIO])
+      msg.liveParameters.stiffnessFactorStd = float(P[States.STIFFNESS])
+      msg.liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET])
+      msg.liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST])
 
       if sm.frame % 1200 == 0:  # once a minute
         params = {
diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript
index ca8dc8285..dfa3dca71 100644
--- a/selfdrive/loggerd/SConscript
+++ b/selfdrive/loggerd/SConscript
@@ -28,4 +28,4 @@ env.Program(src, LIBS=libs)
 env.Program('bootlog.cc', LIBS=libs)
 
 if GetOption('test'):
-  env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc'], LIBS=[libs])
+  env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')], LIBS=[libs] + ['curl'])
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index 2634c69fe..38f959630 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -64,6 +64,7 @@ const LogCameraInfo cameras_logged[] = {
     .has_qcamera = true,
     .trigger_rotate = true,
     .enable = true,
+    .record = true,
   },
   {
     .type = DriverCam,
@@ -76,7 +77,8 @@ const LogCameraInfo cameras_logged[] = {
     .downscale = false,
     .has_qcamera = false,
     .trigger_rotate = Hardware::TICI(),
-    .enable = !Hardware::PC() && Params().getBool("RecordFront"),
+    .enable = !Hardware::PC(),
+    .record = Params().getBool("RecordFront"),
   },
   {
     .type = WideRoadCam,
@@ -90,6 +92,7 @@ const LogCameraInfo cameras_logged[] = {
     .has_qcamera = false,
     .trigger_rotate = true,
     .enable = Hardware::TICI(),
+    .record = Hardware::TICI(),
   },
 };
 const LogCameraInfo qcam_info = {
@@ -146,7 +149,8 @@ void encoder_thread(const LogCameraInfo &cam_info) {
 
       // main encoder
       encoders.push_back(new Encoder(cam_info.filename, buf_info.width, buf_info.height,
-                                     cam_info.fps, cam_info.bitrate, cam_info.is_h265, cam_info.downscale));
+                                     cam_info.fps, cam_info.bitrate, cam_info.is_h265,
+                                     cam_info.downscale, cam_info.record));
       // qcamera encoder
       if (cam_info.has_qcamera) {
         encoders.push_back(new Encoder(qcam_info.filename, qcam_info.frame_width, qcam_info.frame_height,
@@ -233,7 +237,6 @@ void encoder_thread(const LogCameraInfo &cam_info) {
           eidx.setSegmentNum(cur_seg);
           eidx.setSegmentId(out_id);
           if (lh) {
-            // TODO: this should read cereal/services.h for qlog decimation
             auto bytes = msg.toBytes();
             lh_log(lh, bytes.begin(), bytes.size(), true);
           }
@@ -300,7 +303,16 @@ void rotate_if_needed() {
 } // namespace
 
 int main(int argc, char** argv) {
-  setpriority(PRIO_PROCESS, 0, -20);
+  if (Hardware::EON()) {
+    setpriority(PRIO_PROCESS, 0, -20);
+  } else {
+    int ret;
+    ret = set_core_affinity({0, 1, 2, 3});
+    assert(ret == 0);
+    // TODO: why does this impact camerad timings?
+    //ret = set_realtime_priority(1);
+    //assert(ret == 0);
+  }
 
   clear_locks();
 
diff --git a/selfdrive/loggerd/omx_encoder.cc b/selfdrive/loggerd/omx_encoder.cc
index df5e3d248..3ccbbff57 100644
--- a/selfdrive/loggerd/omx_encoder.cc
+++ b/selfdrive/loggerd/omx_encoder.cc
@@ -156,8 +156,9 @@ static const char* omx_color_fomat_name(uint32_t format) {
 
 // ***** encoder functions *****
 
-OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) {
+OmxEncoder::OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write) {
   this->filename = filename;
+  this->write = write;
   this->width = width;
   this->height = height;
   this->fps = fps;
@@ -503,13 +504,15 @@ void OmxEncoder::encoder_open(const char* path) {
 
     this->wrote_codec_config = false;
   } else {
-    this->of = fopen(this->vid_path, "wb");
-    assert(this->of);
+    if (this->write) {
+      this->of = fopen(this->vid_path, "wb");
+      assert(this->of);
 #ifndef QCOM2
-    if (this->codec_config_len > 0) {
-      fwrite(this->codec_config, this->codec_config_len, 1, this->of);
-    }
+      if (this->codec_config_len > 0) {
+        fwrite(this->codec_config, this->codec_config_len, 1, this->of);
+      }
 #endif
+    }
   }
 
   // create camera lock file
@@ -553,8 +556,10 @@ void OmxEncoder::encoder_close() {
       avio_closep(&this->ofmt_ctx->pb);
       avformat_free_context(this->ofmt_ctx);
     } else {
-      fclose(this->of);
-      this->of = nullptr;
+      if (this->of) {
+        fclose(this->of);
+        this->of = nullptr;
+      }
     }
     unlink(this->lock_path);
   }
diff --git a/selfdrive/loggerd/omx_encoder.h b/selfdrive/loggerd/omx_encoder.h
index d7273872a..74ea17f64 100644
--- a/selfdrive/loggerd/omx_encoder.h
+++ b/selfdrive/loggerd/omx_encoder.h
@@ -15,7 +15,7 @@ extern "C" {
 // OmxEncoder, lossey codec using hardware hevc
 class OmxEncoder : public VideoEncoder {
 public:
-  OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale);
+  OmxEncoder(const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale, bool write = true);
   ~OmxEncoder();
   int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
                    int in_width, int in_height, uint64_t ts);
@@ -39,10 +39,11 @@ private:
   char lock_path[1024];
   bool is_open = false;
   bool dirty = false;
+  bool write = false;
   int counter = 0;
 
   const char* filename;
-  FILE *of;
+  FILE *of = nullptr;
 
   size_t codec_config_len;
   uint8_t *codec_config = NULL;
diff --git a/selfdrive/loggerd/raw_logger.cc b/selfdrive/loggerd/raw_logger.cc
index fc400dfe2..39757b8ff 100644
--- a/selfdrive/loggerd/raw_logger.cc
+++ b/selfdrive/loggerd/raw_logger.cc
@@ -21,9 +21,10 @@ extern "C" {
 #include "selfdrive/common/util.h"
 
 RawLogger::RawLogger(const char* filename, int width, int height, int fps,
-                     int bitrate, bool h265, bool downscale)
-  : filename(filename),
-    fps(fps) {
+                     int bitrate, bool h265, bool downscale, bool write)
+  : filename(filename), fps(fps) {
+
+  // TODO: respect write arg
 
   av_register_all();
   codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
diff --git a/selfdrive/loggerd/raw_logger.h b/selfdrive/loggerd/raw_logger.h
index 7955ef5e1..9cef7ddca 100644
--- a/selfdrive/loggerd/raw_logger.h
+++ b/selfdrive/loggerd/raw_logger.h
@@ -16,7 +16,7 @@ extern "C" {
 class RawLogger : public VideoEncoder {
  public:
   RawLogger(const char* filename, int width, int height, int fps,
-            int bitrate, bool h265, bool downscale);
+            int bitrate, bool h265, bool downscale, bool write = true);
   ~RawLogger();
   int encode_frame(const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
                    int in_width, int in_height, uint64_t ts);
@@ -25,6 +25,7 @@ class RawLogger : public VideoEncoder {
 
 private:
   const char* filename;
+  //bool write;
   int fps;
   int counter = 0;
   bool is_open = false;
diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py
index 1cbd81537..d40c7e1f5 100644
--- a/selfdrive/loggerd/uploader.py
+++ b/selfdrive/loggerd/uploader.py
@@ -260,10 +260,10 @@ def uploader_fn(exit_event):
         time.sleep(60 if offroad else 5)
       continue
 
-    on_wifi = network_type == NetworkType.wifi
+    good_internet = network_type in [NetworkType.wifi, NetworkType.ethernet]
     allow_raw_upload = params.get_bool("UploadRaw")
 
-    d = uploader.next_file_to_upload(with_raw=allow_raw_upload and on_wifi and offroad)
+    d = uploader.next_file_to_upload(with_raw=allow_raw_upload and good_internet and offroad)
     if d is None:  # Nothing to upload
       if allow_sleep:
         time.sleep(60 if offroad else 5)
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index bcbd74f99..49c223a9e 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -43,6 +43,9 @@ def manager_init():
   if params.get_bool("RecordFrontLock"):
     params.put_bool("RecordFront", True)
 
+  if not params.get_bool("DisableRadar_Allow"):
+    params.delete("DisableRadar")
+
   # set unset params
   for k, v in default_params:
     if params.get(k) is None:
diff --git a/selfdrive/manager/process.py b/selfdrive/manager/process.py
index a5f91c935..8cb3505b8 100644
--- a/selfdrive/manager/process.py
+++ b/selfdrive/manager/process.py
@@ -160,6 +160,7 @@ class ManagerProcess(ABC):
     state.name = self.name
     if self.proc:
       state.running = self.proc.is_alive()
+      state.shouldBeRunning = self.proc is not None and not self.shutting_down
       state.pid = self.proc.pid or 0
       state.exitCode = self.proc.exitcode or 0
     return state
diff --git a/selfdrive/manager/test/test_manager.py b/selfdrive/manager/test/test_manager.py
old mode 100644
new mode 100755
diff --git a/selfdrive/modeld/constants.py b/selfdrive/modeld/constants.py
index 794e3b37d..125864b98 100644
--- a/selfdrive/modeld/constants.py
+++ b/selfdrive/modeld/constants.py
@@ -1,7 +1,7 @@
 IDX_N = 33
 
-def index_function(idx, max_val=192):
-  return (max_val/1024)*(idx**2)
+def index_function(idx, max_val=192, max_idx=32):
+  return (max_val) * ((idx/max_idx)**2)
 
 
 T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
diff --git a/selfdrive/modeld/dmonitoringmodeld b/selfdrive/modeld/dmonitoringmodeld
index 506f73163..9acd7bcc3 100755
--- a/selfdrive/modeld/dmonitoringmodeld
+++ b/selfdrive/modeld/dmonitoringmodeld
@@ -5,13 +5,13 @@ cd $DIR
 
 if [ -d /system ]; then
   if [ -f /TICI ]; then # QCOM2
-    export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$LD_LIBRARY_PATH"
+    export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
   else # QCOM
-    export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH"
+    export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$LD_LIBRARY_PATH"
   fi
-  export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/dsp/"
+  export ADSP_LIBRARY_PATH="/data/pythonpath/third_party/snpe/dsp/"
 else
   # PC
-  export LD_LIBRARY_PATH="$DIR/../../phonelibs/snpe/x86_64-linux-clang:$DIR/../../openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH"
+  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
 fi
 exec ./_dmonitoringmodeld
diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld
index f3a7c2408..fcf2812cb 100755
--- a/selfdrive/modeld/modeld
+++ b/selfdrive/modeld/modeld
@@ -5,12 +5,12 @@ cd $DIR
 
 if [ -d /system ]; then
   if [ -f /TICI ]; then # QCOM2
-    export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/phonelibs/snpe/larch64:$LD_LIBRARY_PATH"
+    export LD_LIBRARY_PATH="/usr/lib/aarch64-linux-gnu:/data/pythonpath/third_party/snpe/larch64:$LD_LIBRARY_PATH"
   else # QCOM
-    export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64/:$LD_LIBRARY_PATH"
+    export LD_LIBRARY_PATH="/data/pythonpath/third_party/snpe/aarch64/:$LD_LIBRARY_PATH"
   fi
 else
   # PC
-  export LD_LIBRARY_PATH="$DIR/../../phonelibs/snpe/x86_64-linux-clang:$DIR/../../openpilot/phonelibs/snpe/x86_64:$LD_LIBRARY_PATH"
+  export LD_LIBRARY_PATH="$DIR/../../third_party/snpe/x86_64-linux-clang:$DIR/../../openpilot/third_party/snpe/x86_64:$LD_LIBRARY_PATH"
 fi
 exec ./_modeld
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 460e33c56..ef9d59bf3 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -131,13 +131,12 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
 }
 
 int main(int argc, char **argv) {
-  set_realtime_priority(54);
+  int ret;
+  ret = set_realtime_priority(54);
+  assert(ret == 0);
+  set_core_affinity({Hardware::EON() ? 2 : 7});
+  assert(ret == 0);
 
-  if (Hardware::EON()) {
-    set_core_affinity(2);
-  } else if (Hardware::TICI()) {
-    set_core_affinity(7);  
-  }
   bool wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
 
   // start calibration thread
diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc
index 9def7e49e..457519994 100644
--- a/selfdrive/modeld/models/commonmodel.cc
+++ b/selfdrive/modeld/models/commonmodel.cc
@@ -22,16 +22,24 @@ ModelFrame::ModelFrame(cl_device_id device_id, cl_context context) {
   loadyuv_init(&loadyuv, context, device_id, MODEL_WIDTH, MODEL_HEIGHT);
 }
 
-float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, const mat3 &transform) {
+float* ModelFrame::prepare(cl_mem yuv_cl, int frame_width, int frame_height, const mat3 &transform, cl_mem *output) {
   transform_queue(&this->transform, q,
                   yuv_cl, frame_width, frame_height,
                   y_cl, u_cl, v_cl, MODEL_WIDTH, MODEL_HEIGHT, transform);
-  loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
 
-  std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
-  clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr);
-  clFinish(q);
-  return &input_frames[0];
+  if (output == NULL) {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, net_input_cl);
+
+    std::memmove(&input_frames[0], &input_frames[MODEL_FRAME_SIZE], sizeof(float) * MODEL_FRAME_SIZE);
+    CL_CHECK(clEnqueueReadBuffer(q, net_input_cl, CL_TRUE, 0, MODEL_FRAME_SIZE * sizeof(float), &input_frames[MODEL_FRAME_SIZE], 0, nullptr, nullptr));
+    clFinish(q);
+    return &input_frames[0];
+  } else {
+    loadyuv_queue(&loadyuv, q, y_cl, u_cl, v_cl, *output, true);
+    // NOTE: Since thneed is using a different command queue, this clFinish is needed to ensure the image is ready.
+    clFinish(q);
+    return NULL;
+  }
 }
 
 ModelFrame::~ModelFrame() {
diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h
index e4e6563d7..d7904489b 100644
--- a/selfdrive/modeld/models/commonmodel.h
+++ b/selfdrive/modeld/models/commonmodel.h
@@ -30,7 +30,7 @@ class ModelFrame {
  public:
   ModelFrame(cl_device_id device_id, cl_context context);
   ~ModelFrame();
-  float* prepare(cl_mem yuv_cl, int width, int height, const mat3& transform);
+  float* prepare(cl_mem yuv_cl, int width, int height, const mat3& transform, cl_mem *output);
 
   const int buf_size = MODEL_FRAME_SIZE * 2;
 
diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc
index 2711db45e..719a9d8a0 100644
--- a/selfdrive/modeld/models/dmonitoring.cc
+++ b/selfdrive/modeld/models/dmonitoring.cc
@@ -57,18 +57,21 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   if (Hardware::TICI()) {
     const int full_width_tici = 1928;
     const int full_height_tici = 1208;
-    const int adapt_width_tici = 668;
+    const int adapt_width_tici = 954;
+    const int x_offset_tici = -72;
+    const int y_offset_tici = -144;
     const int cropped_height = adapt_width_tici / 1.33;
-    crop_rect = {full_width_tici / 2 - adapt_width_tici / 2,
-                 full_height_tici / 2 - cropped_height / 2 - 196,
+    crop_rect = {full_width_tici / 2 - adapt_width_tici / 2 + x_offset_tici,
+                 full_height_tici / 2 - cropped_height / 2 + y_offset_tici,
                  cropped_height / 2,
                  cropped_height};
     if (!s->is_rhd) {
-      crop_rect.x += adapt_width_tici - crop_rect.w + 32;
+      crop_rect.x += adapt_width_tici - crop_rect.w;
     }
 
   } else {
-    crop_rect = {0, 0, height / 2, height};
+    const int adapt_width = 372;
+    crop_rect = {0, 0, adapt_width, height};
     if (!s->is_rhd) {
       crop_rect.x += width - crop_rect.w;
     }
@@ -95,7 +98,8 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height);
   uint8_t *resized_y = resized_buf;
   libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear;
-  libyuv::I420Scale(cropped_y, crop_rect.w,
+  if (Hardware::TICI()) {
+    libyuv::I420Scale(cropped_y, crop_rect.w,
                     cropped_u, crop_rect.w / 2,
                     cropped_v, crop_rect.w / 2,
                     crop_rect.w, crop_rect.h,
@@ -104,6 +108,21 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
                     resized_v, resized_width / 2,
                     resized_width, resized_height,
                     mode);
+  } else {
+    const int source_height = 0.7*resized_height;
+    const int extra_height = (resized_height - source_height) / 2;
+    const int extra_width = (resized_width - source_height / 2) / 2;
+    const int source_width = source_height / 2 + extra_width;
+    libyuv::I420Scale(cropped_y, crop_rect.w,
+                    cropped_u, crop_rect.w / 2,
+                    cropped_v, crop_rect.w / 2,
+                    crop_rect.w, crop_rect.h,
+                    resized_y + extra_height * resized_width, resized_width,
+                    resized_u + extra_height / 2 * resized_width / 2, resized_width / 2,
+                    resized_v + extra_height / 2 * resized_width / 2, resized_width / 2,
+                    source_width, source_height,
+                    mode);
+  }
 
   int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
   float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
@@ -128,7 +147,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
 
   //printf("preprocess completed. %d \n", yuv_buf_len);
   //FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb");
-  //fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file);
+  //fwrite(resized_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
   //fclose(dump_yuv_file);
 
   // *** testing ***
@@ -162,6 +181,7 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   ret.partial_face = s->output[35];
   ret.distracted_pose = s->output[36];
   ret.distracted_eyes = s->output[37];
+  ret.occluded_prob = s->output[38];
   ret.dsp_execution_time = (t2 - t1) / 1000.;
   return ret;
 }
@@ -188,6 +208,7 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
   framed.setPartialFace(res.partial_face);
   framed.setDistractedPose(res.distracted_pose);
   framed.setDistractedEyes(res.distracted_eyes);
+  framed.setOccludedProb(res.occluded_prob);
   if (send_raw_pred) {
     framed.setRawPredictions(raw_pred.asBytes());
   }
diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h
index 301537473..9b3ee631f 100644
--- a/selfdrive/modeld/models/dmonitoring.h
+++ b/selfdrive/modeld/models/dmonitoring.h
@@ -7,7 +7,7 @@
 #include "selfdrive/modeld/models/commonmodel.h"
 #include "selfdrive/modeld/runners/run.h"
 
-#define OUTPUT_SIZE 38
+#define OUTPUT_SIZE 39
 
 typedef struct DMonitoringResult {
   float face_orientation[3];
@@ -24,6 +24,7 @@ typedef struct DMonitoringResult {
   float partial_face;
   float distracted_pose;
   float distracted_eyes;
+  float occluded_prob;
   float dsp_execution_time;
 } DMonitoringResult;
 
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index 0ebb9a374..2be6e75fb 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -12,42 +12,6 @@
 #include "selfdrive/common/params.h"
 #include "selfdrive/common/timing.h"
 
-constexpr int DESIRE_PRED_SIZE = 32;
-constexpr int OTHER_META_SIZE = 32;
-constexpr int NUM_META_INTERVALS = 5;
-constexpr int META_STRIDE = 6;
-
-constexpr int PLAN_MHP_N = 5;
-constexpr int PLAN_MHP_COLUMNS = 15;
-constexpr int PLAN_MHP_VALS = 15*33;
-constexpr int PLAN_MHP_SELECTION = 1;
-constexpr int PLAN_MHP_GROUP_SIZE =  (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
-
-constexpr int LEAD_MHP_N = 5;
-constexpr int LEAD_TRAJ_LEN = 6;
-constexpr int LEAD_PRED_DIM = 4;
-constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
-constexpr int LEAD_MHP_SELECTION = 3;
-constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
-
-constexpr int POSE_SIZE = 12;
-
-constexpr int PLAN_IDX = 0;
-constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
-constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
-constexpr int RE_IDX = LL_PROB_IDX + 8;
-constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
-constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
-constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
-constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
-constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
-constexpr int OUTPUT_SIZE =  POSE_IDX + POSE_SIZE;
-#ifdef TEMPORAL
-  constexpr int TEMPORAL_SIZE = 512;
-#else
-  constexpr int TEMPORAL_SIZE = 0;
-#endif
-
 constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
 constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
 constexpr float FCW_THRESHOLD_3MS2 = 0.7;
@@ -57,18 +21,20 @@ float prev_brake_3ms2_probs[3] = {0,0,0};
 
 // #define DUMP_YUV
 
+template<class T, size_t size>
+constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
+  return kj::ArrayPtr(arr.data(), arr.size());
+}
+
 void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   s->frame = new ModelFrame(device_id, context);
 
-  constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
-  s->output.resize(output_size);
-
 #ifdef USE_THNEED
-  s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed", &s->output[0], output_size, USE_GPU_RUNTIME);
+  s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
 #elif USE_ONNX_MODEL
-  s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx", &s->output[0], output_size, USE_GPU_RUNTIME);
+  s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
 #else
-  s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME);
+  s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
 #endif
 
 #ifdef TEMPORAL
@@ -103,21 +69,21 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
   }
 #endif
 
-  //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
+  //for (int i = 0; i < NET_OUTPUT_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
 
-  auto net_input_buf = s->frame->prepare(yuv_cl, width, height, transform);
+  // if getInputBuf is not NULL, net_input_buf will be
+  auto net_input_buf = s->frame->prepare(yuv_cl, width, height, transform, static_cast<cl_mem*>(s->m->getInputBuf()));
   s->m->execute(net_input_buf, s->frame->buf_size);
 
   // net outputs
-  ModelDataRaw net_outputs;
-  net_outputs.plan = &s->output[PLAN_IDX];
-  net_outputs.lane_lines = &s->output[LL_IDX];
-  net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
-  net_outputs.road_edges = &s->output[RE_IDX];
-  net_outputs.lead = &s->output[LEAD_IDX];
-  net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
-  net_outputs.meta = &s->output[DESIRE_STATE_IDX];
-  net_outputs.pose = &s->output[POSE_IDX];
+  ModelDataRaw net_outputs {
+    .plans = (ModelDataRawPlans*)&s->output[PLAN_IDX],
+    .lane_lines = (ModelDataRawLaneLines*)&s->output[LL_IDX],
+    .road_edges = (ModelDataRawRoadEdges*)&s->output[RE_IDX],
+    .leads = (ModelDataRawLeads*)&s->output[LEAD_IDX],
+    .meta = &s->output[DESIRE_STATE_IDX],
+    .pose = (ModelDataRawPose*)&s->output[POSE_IDX],
+  };
   return net_outputs;
 }
 
@@ -125,64 +91,38 @@ void model_free(ModelState* s) {
   delete s->frame;
 }
 
-static const float *get_best_data(const float *data, int size, int group_size, int offset) {
-  int max_idx = 0;
-  for (int i = 1; i < size; i++) {
-    if (data[(i + 1) * group_size + offset] >
-        data[(max_idx + 1) * group_size + offset]) {
-      max_idx = i;
-    }
-  }
-  return &data[max_idx * group_size];
-}
-
-static const float *get_plan_data(float *plan) {
-  return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, -1);
-}
-
-static const float *get_lead_data(const float *lead, int t_offset) {
-  return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
-}
-
-
 void fill_sigmoid(const float *input, float *output, int len, int stride) {
   for (int i=0; i<len; i++) {
     output[i] = sigmoid(input[i*stride]);
   }
 }
 
-void fill_lead_v3(cereal::ModelDataV2::LeadDataV3::Builder lead, const float *lead_data, const float *prob, int t_offset, float prob_t) {
-  float t[LEAD_TRAJ_LEN] = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
-  const float *data = get_lead_data(lead_data, t_offset);
-  lead.setProb(sigmoid(prob[t_offset]));
+void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelDataRawLeads &leads, int t_idx, float prob_t) {
+  std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
+  auto best_prediction = leads.get_best_prediction(t_idx);
+  lead.setProb(sigmoid(leads.prob[t_idx]));
   lead.setProbTime(prob_t);
-  float x_arr[LEAD_TRAJ_LEN];
-  float y_arr[LEAD_TRAJ_LEN];
-  float v_arr[LEAD_TRAJ_LEN];
-  float a_arr[LEAD_TRAJ_LEN];
-  float x_stds_arr[LEAD_TRAJ_LEN];
-  float y_stds_arr[LEAD_TRAJ_LEN];
-  float v_stds_arr[LEAD_TRAJ_LEN];
-  float a_stds_arr[LEAD_TRAJ_LEN];
+  std::array<float, LEAD_TRAJ_LEN> lead_x, lead_y, lead_v, lead_a;
+  std::array<float, LEAD_TRAJ_LEN> lead_x_std, lead_y_std, lead_v_std, lead_a_std;
   for (int i=0; i<LEAD_TRAJ_LEN; i++) {
-    x_arr[i] = data[i*LEAD_PRED_DIM+0];
-    y_arr[i] = data[i*LEAD_PRED_DIM+1];
-    v_arr[i] = data[i*LEAD_PRED_DIM+2];
-    a_arr[i] = data[i*LEAD_PRED_DIM+3];
-    x_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+0]);
-    y_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+1]);
-    v_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+2]);
-    a_stds_arr[i] = exp(data[LEAD_MHP_VALS + i*LEAD_PRED_DIM+3]);
+    lead_x[i] = best_prediction.mean[i].x;
+    lead_y[i] = best_prediction.mean[i].y;
+    lead_v[i] = best_prediction.mean[i].velocity;
+    lead_a[i] = best_prediction.mean[i].acceleration;
+    lead_x_std[i] = exp(best_prediction.std[i].x);
+    lead_y_std[i] = exp(best_prediction.std[i].y);
+    lead_v_std[i] = exp(best_prediction.std[i].velocity);
+    lead_a_std[i] = exp(best_prediction.std[i].acceleration);
   }
-  lead.setT(t);
-  lead.setX(x_arr);
-  lead.setY(y_arr);
-  lead.setV(v_arr);
-  lead.setA(a_arr);
-  lead.setXStd(x_stds_arr);
-  lead.setYStd(y_stds_arr);
-  lead.setVStd(v_stds_arr);
-  lead.setAStd(a_stds_arr);
+  lead.setT(to_kj_array_ptr(lead_t));
+  lead.setX(to_kj_array_ptr(lead_x));
+  lead.setY(to_kj_array_ptr(lead_y));
+  lead.setV(to_kj_array_ptr(lead_v));
+  lead.setA(to_kj_array_ptr(lead_a));
+  lead.setXStd(to_kj_array_ptr(lead_x_std));
+  lead.setYStd(to_kj_array_ptr(lead_y_std));
+  lead.setVStd(to_kj_array_ptr(lead_v_std));
+  lead.setAStd(to_kj_array_ptr(lead_a_std));
 }
 
 void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_data) {
@@ -207,6 +147,7 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
   fill_sigmoid(&meta_data[DESIRE_LEN+4], brake_3ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
   fill_sigmoid(&meta_data[DESIRE_LEN+5], brake_4ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
   fill_sigmoid(&meta_data[DESIRE_LEN+6], brake_5ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
+  //fill_sigmoid(&meta_data[DESIRE_LEN+7], GAS PRESSED, NUM_META_INTERVALS, META_STRIDE);
 
   std::memmove(prev_brake_5ms2_probs, &prev_brake_5ms2_probs[1], 4*sizeof(float));
   std::memmove(prev_brake_3ms2_probs, &prev_brake_3ms2_probs[1], 2*sizeof(float));
@@ -237,100 +178,150 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
   meta.setHardBrakePredicted(above_fcw_threshold);
 }
 
-void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
-               int columns, int column_offset, float * plan_t_arr, bool fill_std) {
-  float x_arr[TRAJECTORY_SIZE] = {};
-  float y_arr[TRAJECTORY_SIZE] = {};
-  float z_arr[TRAJECTORY_SIZE] = {};
-  float x_std_arr[TRAJECTORY_SIZE];
-  float y_std_arr[TRAJECTORY_SIZE];
-  float z_std_arr[TRAJECTORY_SIZE];
-  float t_arr[TRAJECTORY_SIZE];
-  for (int i=0; i<TRAJECTORY_SIZE; i++) {
-    // column_offset == -1 means this data is X indexed not T indexed
-    if (column_offset >= 0) {
-      t_arr[i] = T_IDXS[i];
-      x_arr[i] = data[i*columns + 0 + column_offset];
-      x_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 0 + column_offset];
-    } else {
-      t_arr[i] = plan_t_arr[i];
-      x_arr[i] = X_IDXS[i];
-      x_std_arr[i] = NAN;
-    }
-    y_arr[i] = data[i*columns + 1 + column_offset];
-    y_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 1 + column_offset];
-    z_arr[i] = data[i*columns + 2 + column_offset];
-    z_std_arr[i] = data[columns*(TRAJECTORY_SIZE + i) + 2 + column_offset];
+template<size_t size>
+void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
+  xyzt.setT(to_kj_array_ptr(t));
+  xyzt.setX(to_kj_array_ptr(x));
+  xyzt.setY(to_kj_array_ptr(y));
+  xyzt.setZ(to_kj_array_ptr(z));
+}
+
+template<size_t size>
+void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
+               const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
+  fill_xyzt(xyzt, t, x, y, z);
+  xyzt.setXStd(to_kj_array_ptr(x_std));
+  xyzt.setYStd(to_kj_array_ptr(y_std));
+  xyzt.setZStd(to_kj_array_ptr(z_std));
+}
+
+void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPrediction &plan) {
+  std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
+  std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
+  std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
+  std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
+  std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
+
+  for(int i=0; i<TRAJECTORY_SIZE; i++) {
+    pos_x[i] = plan.mean[i].position.x;
+    pos_y[i] = plan.mean[i].position.y;
+    pos_z[i] = plan.mean[i].position.z;
+    pos_x_std[i] = exp(plan.std[i].position.x);
+    pos_y_std[i] = exp(plan.std[i].position.y);
+    pos_z_std[i] = exp(plan.std[i].position.z);
+    vel_x[i] = plan.mean[i].velocity.x;
+    vel_y[i] = plan.mean[i].velocity.y;
+    vel_z[i] = plan.mean[i].velocity.z;
+    rot_x[i] = plan.mean[i].rotation.x;
+    rot_y[i] = plan.mean[i].rotation.y;
+    rot_z[i] = plan.mean[i].rotation.z;
+    rot_rate_x[i] = plan.mean[i].rotation_rate.x;
+    rot_rate_y[i] = plan.mean[i].rotation_rate.y;
+    rot_rate_z[i] = plan.mean[i].rotation_rate.z;
   }
-  xyzt.setX(x_arr);
-  xyzt.setY(y_arr);
-  xyzt.setZ(z_arr);
-  xyzt.setT(t_arr);
-  if (fill_std) {
-    xyzt.setXStd(x_std_arr);
-    xyzt.setYStd(y_std_arr);
-    xyzt.setZStd(z_std_arr);
+
+  fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
+  fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
+  fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
+  fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
+}
+
+void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
+                     const ModelDataRawLaneLines &lanes) {
+  std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
+  std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
+  std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
+  std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
+  for (int j=0; j<TRAJECTORY_SIZE; j++) {
+    left_far_y[j] = lanes.mean.left_far[j].y;
+    left_far_z[j] = lanes.mean.left_far[j].z;
+    left_near_y[j] = lanes.mean.left_near[j].y;
+    left_near_z[j] = lanes.mean.left_near[j].z;
+    right_near_y[j] = lanes.mean.right_near[j].y;
+    right_near_z[j] = lanes.mean.right_near[j].z;
+    right_far_y[j] = lanes.mean.right_far[j].y;
+    right_far_z[j] = lanes.mean.right_far[j].z;
   }
+
+  auto lane_lines = framed.initLaneLines(4);
+  fill_xyzt(lane_lines[0], plan_t, X_IDXS_FLOAT, left_far_y, left_far_z);
+  fill_xyzt(lane_lines[1], plan_t, X_IDXS_FLOAT, left_near_y, left_near_z);
+  fill_xyzt(lane_lines[2], plan_t, X_IDXS_FLOAT, right_near_y, right_near_z);
+  fill_xyzt(lane_lines[3], plan_t, X_IDXS_FLOAT, right_far_y, right_far_z);
+
+  framed.setLaneLineStds({
+    exp(lanes.std.left_far[0].y),
+    exp(lanes.std.left_near[0].y),
+    exp(lanes.std.right_near[0].y),
+    exp(lanes.std.right_far[0].y),
+  });
+
+  framed.setLaneLineProbs({
+    sigmoid(lanes.prob.left_far.val),
+    sigmoid(lanes.prob.left_near.val),
+    sigmoid(lanes.prob.right_near.val),
+    sigmoid(lanes.prob.right_far.val),
+  });
+}
+
+void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
+                     const ModelDataRawRoadEdges &edges) {
+  std::array<float, TRAJECTORY_SIZE> left_y, left_z;
+  std::array<float, TRAJECTORY_SIZE> right_y, right_z;
+  for (int j=0; j<TRAJECTORY_SIZE; j++) {
+    left_y[j] = edges.mean.left[j].y;
+    left_z[j] = edges.mean.left[j].z;
+    right_y[j] = edges.mean.right[j].y;
+    right_z[j] = edges.mean.right[j].z;
+  }
+
+  auto road_edges = framed.initRoadEdges(2);
+  fill_xyzt(road_edges[0], plan_t, X_IDXS_FLOAT, left_y, left_z);
+  fill_xyzt(road_edges[1], plan_t, X_IDXS_FLOAT, right_y, right_z);
+
+  framed.setRoadEdgeStds({
+    exp(edges.std.left[0].y),
+    exp(edges.std.right[0].y),
+  });
 }
 
 void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
-  // plan
-  const float *best_plan = get_plan_data(net_outputs.plan);
-  float plan_t_arr[TRAJECTORY_SIZE];
-  std::fill_n(plan_t_arr, TRAJECTORY_SIZE, NAN);
-  plan_t_arr[0] = 0.0;
+  auto best_plan = net_outputs.plans->get_best_prediction();
+  std::array<float, TRAJECTORY_SIZE> plan_t;
+  std::fill_n(plan_t.data(), plan_t.size(), NAN);
+  plan_t[0] = 0.0;
   for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
     // increment tidx until we find an element that's further away than the current xidx
-    while (tidx < TRAJECTORY_SIZE-1 && best_plan[(tidx+1)*PLAN_MHP_COLUMNS] < X_IDXS[xidx]) {
+    for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
       tidx++;
     }
-    float current_x_val = best_plan[tidx*PLAN_MHP_COLUMNS];
-    float next_x_val = best_plan[(tidx+1)*PLAN_MHP_COLUMNS];
-    if (next_x_val < X_IDXS[xidx]) {
-      // if the plan doesn't extend far enough, set plan_t to the max value (10s), then break
-      plan_t_arr[xidx] = T_IDXS[TRAJECTORY_SIZE-1];
+    if (tidx == TRAJECTORY_SIZE - 1) {
+      // if the Plan doesn't extend far enough, set plan_t to the max value (10s), then break
+      plan_t[xidx] = T_IDXS[TRAJECTORY_SIZE - 1];
       break;
-    } else {
-      // otherwise, interpolate to find `t` for the current xidx
-      float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
-      plan_t_arr[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
     }
+
+    // interpolate to find `t` for the current xidx
+    float current_x_val = best_plan.mean[tidx].position.x;
+    float next_x_val = best_plan.mean[tidx+1].position.x;
+    float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
+    plan_t[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
   }
 
-  fill_xyzt(framed.initPosition(), best_plan, PLAN_MHP_COLUMNS, 0, plan_t_arr, true);
-  fill_xyzt(framed.initVelocity(), best_plan, PLAN_MHP_COLUMNS, 3, plan_t_arr, false);
-  fill_xyzt(framed.initOrientation(), best_plan, PLAN_MHP_COLUMNS, 9, plan_t_arr, false);
-  fill_xyzt(framed.initOrientationRate(), best_plan, PLAN_MHP_COLUMNS, 12, plan_t_arr, false);
-
-  // lane lines
-  auto lane_lines = framed.initLaneLines(4);
-  float lane_line_probs_arr[4];
-  float lane_line_stds_arr[4];
-  for (int i = 0; i < 4; i++) {
-    fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false);
-    lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i*2+1]);
-    lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
-  }
-  framed.setLaneLineProbs(lane_line_probs_arr);
-  framed.setLaneLineStds(lane_line_stds_arr);
-
-  // road edges
-  auto road_edges = framed.initRoadEdges(2);
-  float road_edge_stds_arr[2];
-  for (int i = 0; i < 2; i++) {
-    fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2], 2, -1, plan_t_arr, false);
-    road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
-  }
-  framed.setRoadEdgeStds(road_edge_stds_arr);
+  fill_plan(framed, best_plan);
+  fill_lane_lines(framed, plan_t, *net_outputs.lane_lines);
+  fill_road_edges(framed, plan_t, *net_outputs.road_edges);
 
   // meta
   fill_meta(framed.initMeta(), net_outputs.meta);
 
   // leads
   auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
-  float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
-  for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
-    fill_lead_v3(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
+  std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
+  for (int i=0; i<LEAD_MHP_SELECTION; i++) {
+    fill_lead(leads[i], *net_outputs.leads, i, t_offsets[i]);
   }
 }
 
@@ -354,25 +345,17 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
 
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
                      const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
-  float trans_arr[3];
-  float trans_std_arr[3];
-  float rot_arr[3];
-  float rot_std_arr[3];
-
-  for (int i =0; i < 3; i++) {
-    trans_arr[i] = net_outputs.pose[i];
-    trans_std_arr[i] = exp(net_outputs.pose[6 + i]);
-
-    rot_arr[i] = net_outputs.pose[3 + i];
-    rot_std_arr[i] = exp(net_outputs.pose[9 + i]);
-  }
-
   MessageBuilder msg;
+  auto v_mean = net_outputs.pose->velocity_mean;
+  auto r_mean = net_outputs.pose->rotation_mean;
+  auto v_std = net_outputs.pose->velocity_std;
+  auto r_std = net_outputs.pose->rotation_std;
+
   auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
-  posenetd.setTrans(trans_arr);
-  posenetd.setRot(rot_arr);
-  posenetd.setTransStd(trans_std_arr);
-  posenetd.setRotStd(rot_std_arr);
+  posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
+  posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
+  posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
+  posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
 
   posenetd.setTimestampEof(timestamp_eof);
   posenetd.setFrameId(vipc_frame_id);
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index 0840483eb..c6d3e00ac 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -5,6 +5,7 @@
 #define DESIRE
 #define TRAFFIC_CONVENTION
 
+#include <array>
 #include <memory>
 
 #include "cereal/messaging/messaging.h"
@@ -18,22 +19,181 @@ constexpr int DESIRE_LEN = 8;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
 constexpr int MODEL_FREQ = 20;
 
+constexpr int DESIRE_PRED_SIZE = 32;
+constexpr int OTHER_META_SIZE = 48;
+constexpr int NUM_META_INTERVALS = 5;
+constexpr int META_STRIDE = 7;
+
+constexpr int PLAN_MHP_N = 5;
+constexpr int PLAN_MHP_VALS = 15*33;
+constexpr int PLAN_MHP_SELECTION = 1;
+constexpr int PLAN_MHP_GROUP_SIZE =  (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
+
+constexpr int LEAD_MHP_N = 2;
+constexpr int LEAD_TRAJ_LEN = 6;
+constexpr int LEAD_PRED_DIM = 4;
+constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
+constexpr int LEAD_MHP_SELECTION = 3;
+constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
+
+constexpr int POSE_SIZE = 12;
+
+constexpr int PLAN_IDX = 0;
+constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
+constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
+constexpr int RE_IDX = LL_PROB_IDX + 8;
+constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
+constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
+constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
+constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
+constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
+constexpr int OUTPUT_SIZE =  POSE_IDX + POSE_SIZE;
+#ifdef TEMPORAL
+  constexpr int TEMPORAL_SIZE = 512;
+#else
+  constexpr int TEMPORAL_SIZE = 0;
+#endif
+constexpr int NET_OUTPUT_SIZE =  OUTPUT_SIZE + TEMPORAL_SIZE;
+
+struct ModelDataRawXYZ {
+  float x;
+  float y;
+  float z;
+};
+static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
+
+struct ModelDataRawYZ {
+  float y;
+  float z;
+};
+static_assert(sizeof(ModelDataRawYZ) == sizeof(float)*2);
+
+struct ModelDataRawPlanElement {
+  ModelDataRawXYZ position;
+  ModelDataRawXYZ velocity;
+  ModelDataRawXYZ acceleration;
+  ModelDataRawXYZ rotation;
+  ModelDataRawXYZ rotation_rate;
+};
+static_assert(sizeof(ModelDataRawPlanElement) == sizeof(ModelDataRawXYZ)*5);
+
+struct ModelDataRawPlanPrediction {
+  std::array<ModelDataRawPlanElement, TRAJECTORY_SIZE> mean;
+  std::array<ModelDataRawPlanElement, TRAJECTORY_SIZE> std;
+  float prob;
+};
+static_assert(sizeof(ModelDataRawPlanPrediction) == (sizeof(ModelDataRawPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
+
+struct ModelDataRawPlans {
+  std::array<ModelDataRawPlanPrediction, PLAN_MHP_N> prediction;
+
+  constexpr const ModelDataRawPlanPrediction &get_best_prediction() const {
+    int max_idx = 0;
+    for (int i = 1; i < prediction.size(); i++) {
+      if (prediction[i].prob > prediction[max_idx].prob) {
+        max_idx = i;
+      }
+    }
+    return prediction[max_idx];
+  }
+};
+static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPrediction)*PLAN_MHP_N);
+
+struct ModelDataRawLinesXY {
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left_far;
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left_near;
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right_near;
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right_far;
+};
+static_assert(sizeof(ModelDataRawLinesXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*4);
+
+struct ModelDataRawLineProbVal {
+  float val_deprecated;
+  float val;
+};
+static_assert(sizeof(ModelDataRawLineProbVal) == sizeof(float)*2);
+
+struct ModelDataRawLinesProb {
+  ModelDataRawLineProbVal left_far;
+  ModelDataRawLineProbVal left_near;
+  ModelDataRawLineProbVal right_near;
+  ModelDataRawLineProbVal right_far;
+};
+static_assert(sizeof(ModelDataRawLinesProb) == sizeof(ModelDataRawLineProbVal)*4);
+
+struct ModelDataRawLaneLines {
+  ModelDataRawLinesXY mean;
+  ModelDataRawLinesXY std;
+  ModelDataRawLinesProb prob;
+};
+static_assert(sizeof(ModelDataRawLaneLines) == (sizeof(ModelDataRawLinesXY)*2) + sizeof(ModelDataRawLinesProb));
+
+struct ModelDataRawEdgessXY {
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left;
+  std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right;
+};
+static_assert(sizeof(ModelDataRawEdgessXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*2);
+
+struct ModelDataRawRoadEdges {
+  ModelDataRawEdgessXY mean;
+  ModelDataRawEdgessXY std;
+};
+static_assert(sizeof(ModelDataRawRoadEdges) == (sizeof(ModelDataRawEdgessXY)*2));
+
+struct ModelDataRawLeadElement {
+  float x;
+  float y;
+  float velocity;
+  float acceleration;
+};
+static_assert(sizeof(ModelDataRawLeadElement) == sizeof(float)*4);
+
+struct ModelDataRawLeadPrediction {
+  std::array<ModelDataRawLeadElement, LEAD_TRAJ_LEN> mean;
+  std::array<ModelDataRawLeadElement, LEAD_TRAJ_LEN> std;
+  std::array<float, LEAD_MHP_SELECTION> prob;
+};
+static_assert(sizeof(ModelDataRawLeadPrediction) == (sizeof(ModelDataRawLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
+
+struct ModelDataRawLeads {
+  std::array<ModelDataRawLeadPrediction, LEAD_MHP_N> prediction;
+  std::array<float, LEAD_MHP_SELECTION> prob;
+
+  constexpr const ModelDataRawLeadPrediction &get_best_prediction(int t_idx) const {
+    int max_idx = 0;
+    for (int i = 1; i < prediction.size(); i++) {
+      if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
+        max_idx = i;
+      }
+    }
+    return prediction[max_idx];
+  }
+};
+static_assert(sizeof(ModelDataRawLeads) == (sizeof(ModelDataRawLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
+
+struct ModelDataRawPose {
+  ModelDataRawXYZ velocity_mean;
+  ModelDataRawXYZ rotation_mean;
+  ModelDataRawXYZ velocity_std;
+  ModelDataRawXYZ rotation_std;
+};
+static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
+
 struct ModelDataRaw {
-  float *plan;
-  float *lane_lines;
-  float *lane_lines_prob;
-  float *road_edges;
-  float *lead;
-  float *lead_prob;
-  float *desire_state;
-  float *meta;
-  float *desire_pred;
-  float *pose;
+  const ModelDataRawPlans *const plans;
+  const ModelDataRawLaneLines *const lane_lines;
+  const ModelDataRawRoadEdges *const road_edges;
+  const ModelDataRawLeads *const leads;
+  const float *const desire_state;
+  const float *const meta;
+  const float *const desire_pred;
+  const ModelDataRawPose *const pose;
 };
 
-typedef struct ModelState {
+// TODO: convert remaining arrays to std::array and update model runners
+struct ModelState {
   ModelFrame *frame;
-  std::vector<float> output;
+  std::array<float, NET_OUTPUT_SIZE> output = {};
   std::unique_ptr<RunModel> m;
 #ifdef DESIRE
   float prev_desire[DESIRE_LEN] = {};
@@ -42,7 +202,7 @@ typedef struct ModelState {
 #ifdef TRAFFIC_CONVENTION
   float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
 #endif
-} ModelState;
+};
 
 void model_init(ModelState* s, cl_device_id device_id, cl_context context);
 ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
diff --git a/selfdrive/modeld/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h
index 9eb7e1ea6..0893a4acc 100644
--- a/selfdrive/modeld/runners/runmodel.h
+++ b/selfdrive/modeld/runners/runmodel.h
@@ -5,5 +5,6 @@ public:
   virtual void addDesire(float *state, int state_size) {}
   virtual void addTrafficConvention(float *state, int state_size) {}
   virtual void execute(float *net_input_buf, int buf_size) {}
+  virtual void* getInputBuf() { return nullptr; }
 };
 
diff --git a/selfdrive/modeld/runners/thneedmodel.cc b/selfdrive/modeld/runners/thneedmodel.cc
index be782b974..0beb6a279 100644
--- a/selfdrive/modeld/runners/thneedmodel.cc
+++ b/selfdrive/modeld/runners/thneedmodel.cc
@@ -25,6 +25,11 @@ void ThneedModel::addDesire(float *state, int state_size) {
   desire = state;
 }
 
+void* ThneedModel::getInputBuf() {
+  if (thneed->input_clmem.size() > 3) return &(thneed->input_clmem[3]);
+  else return nullptr;
+}
+
 void ThneedModel::execute(float *net_input_buf, int buf_size) {
   float *inputs[4] = {recurrent, trafficConvention, desire, net_input_buf};
   if (!recorded) {
diff --git a/selfdrive/modeld/runners/thneedmodel.h b/selfdrive/modeld/runners/thneedmodel.h
index 933c751d9..1720d5ff2 100644
--- a/selfdrive/modeld/runners/thneedmodel.h
+++ b/selfdrive/modeld/runners/thneedmodel.h
@@ -10,6 +10,7 @@ public:
   void addTrafficConvention(float *state, int state_size);
   void addDesire(float *state, int state_size);
   void execute(float *net_input_buf, int buf_size);
+  void* getInputBuf();
 private:
   Thneed *thneed = NULL;
   bool recorded;
diff --git a/selfdrive/modeld/thneed/thneed.cc b/selfdrive/modeld/thneed/thneed.cc
index 89fdfe391..242905d9f 100644
--- a/selfdrive/modeld/thneed/thneed.cc
+++ b/selfdrive/modeld/thneed/thneed.cc
@@ -241,6 +241,7 @@ void Thneed::find_inputs_outputs() {
     for (int i = 0; i < k->num_args; i++) {
       if (k->name == "zero_pad_image_float" && k->arg_names[i] == "input") {
         cl_mem aa = *(cl_mem*)(k->args[i].data());
+        input_clmem.push_back(aa);
 
         size_t sz;
         clGetMemObjectInfo(aa, CL_MEM_SIZE, sizeof(sz), &sz, NULL);
@@ -262,7 +263,7 @@ void Thneed::copy_inputs(float **finputs) {
   //cl_int ret;
   for (int idx = 0; idx < inputs.size(); ++idx) {
     if (record & THNEED_DEBUG) printf("copying %lu -- %p -> %p\n", input_sizes[idx], finputs[idx], inputs[idx]);
-    memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
+    if (finputs[idx] != NULL) memcpy(inputs[idx], finputs[idx], input_sizes[idx]);
   }
 }
 
diff --git a/selfdrive/modeld/thneed/thneed.h b/selfdrive/modeld/thneed/thneed.h
index 1ae724747..77d27e043 100644
--- a/selfdrive/modeld/thneed/thneed.h
+++ b/selfdrive/modeld/thneed/thneed.h
@@ -98,6 +98,7 @@ class Thneed {
     void wait();
     int optimize();
 
+    vector<cl_mem> input_clmem;
     vector<void *> inputs;
     vector<size_t> input_sizes;
     cl_mem output = NULL;
diff --git a/selfdrive/modeld/transforms/loadyuv.cc b/selfdrive/modeld/transforms/loadyuv.cc
index 9f3dd45aa..39f404a89 100644
--- a/selfdrive/modeld/transforms/loadyuv.cc
+++ b/selfdrive/modeld/transforms/loadyuv.cc
@@ -19,6 +19,7 @@ void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int w
 
   s->loadys_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loadys", &err));
   s->loaduv_krnl = CL_CHECK_ERR(clCreateKernel(prg, "loaduv", &err));
+  s->copy_krnl = CL_CHECK_ERR(clCreateKernel(prg, "copy", &err));
 
   // done with this
   CL_CHECK(clReleaseProgram(prg));
@@ -27,33 +28,46 @@ void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int w
 void loadyuv_destroy(LoadYUVState* s) {
   CL_CHECK(clReleaseKernel(s->loadys_krnl));
   CL_CHECK(clReleaseKernel(s->loaduv_krnl));
+  CL_CHECK(clReleaseKernel(s->copy_krnl));
 }
 
 void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
                    cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
-                   cl_mem out_cl) {
+                   cl_mem out_cl, bool do_shift) {
+  cl_int global_out_off = 0;
+  if (do_shift) {
+    // shift the image in slot 1 to slot 0, then place the new image in slot 1
+    global_out_off += (s->width*s->height) + (s->width/2)*(s->height/2)*2;
+    CL_CHECK(clSetKernelArg(s->copy_krnl, 0, sizeof(cl_mem), &out_cl));
+    CL_CHECK(clSetKernelArg(s->copy_krnl, 1, sizeof(cl_int), &global_out_off));
+    const size_t copy_work_size = global_out_off/8;
+    CL_CHECK(clEnqueueNDRangeKernel(q, s->copy_krnl, 1, NULL,
+                                &copy_work_size, NULL, 0, 0, NULL));
+  }
+
   CL_CHECK(clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl));
   CL_CHECK(clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl));
+  CL_CHECK(clSetKernelArg(s->loadys_krnl, 2, sizeof(cl_int), &global_out_off));
 
   const size_t loadys_work_size = (s->width*s->height)/8;
   CL_CHECK(clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
                                &loadys_work_size, NULL, 0, 0, NULL));
 
   const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
-  cl_int loaduv_out_off = (s->width*s->height);
+  global_out_off += (s->width*s->height);
 
   CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl));
   CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
 
   CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
                                &loaduv_work_size, NULL, 0, 0, NULL));
 
-  loaduv_out_off += (s->width/2)*(s->height/2);
+  global_out_off += (s->width/2)*(s->height/2);
 
   CL_CHECK(clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl));
   CL_CHECK(clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl));
-  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off));
+  CL_CHECK(clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &global_out_off));
 
   CL_CHECK(clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
                                &loaduv_work_size, NULL, 0, 0, NULL));
diff --git a/selfdrive/modeld/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl
index fc7655b56..7dd3d973a 100644
--- a/selfdrive/modeld/transforms/loadyuv.cl
+++ b/selfdrive/modeld/transforms/loadyuv.cl
@@ -1,7 +1,8 @@
 #define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
 
 __kernel void loadys(__global uchar8 const * const Y,
-                     __global float * out)
+                     __global float * out,
+                     int out_offset)
 {
     const int gid = get_global_id(0);
     const int ois = gid * 8;
@@ -17,11 +18,11 @@ __kernel void loadys(__global uchar8 const * const Y,
     __global float* outy0;
     __global float* outy1;
     if ((oy & 1) == 0) {
-      outy0 = out; //y0
-      outy1 = out + UV_SIZE*2; //y2
+      outy0 = out + out_offset; //y0
+      outy1 = out + out_offset + UV_SIZE*2; //y2
     } else {
-      outy0 = out + UV_SIZE; //y1
-      outy1 = out + UV_SIZE*3; //y3
+      outy0 = out + out_offset + UV_SIZE; //y1
+      outy1 = out + out_offset + UV_SIZE*3; //y3
     }
 
     vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
@@ -37,3 +38,10 @@ __kernel void loaduv(__global uchar8 const * const in,
   const float8 outv  = convert_float8(inv);
   out[gid + out_offset / 8] = outv;
 }
+
+__kernel void copy(__global float8 * inout,
+                   int in_offset)
+{
+  const int gid = get_global_id(0);
+  inout[gid] = inout[gid + in_offset / 8];
+}
diff --git a/selfdrive/modeld/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h
index a77258deb..a3161b293 100644
--- a/selfdrive/modeld/transforms/loadyuv.h
+++ b/selfdrive/modeld/transforms/loadyuv.h
@@ -4,7 +4,7 @@
 
 typedef struct {
   int width, height;
-  cl_kernel loadys_krnl, loaduv_krnl;
+  cl_kernel loadys_krnl, loaduv_krnl, copy_krnl;
 } LoadYUVState;
 
 void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height);
@@ -13,4 +13,4 @@ void loadyuv_destroy(LoadYUVState* s);
 
 void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
                    cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
-                   cl_mem out_cl);
+                   cl_mem out_cl, bool do_shift = false);
diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py
index 266870948..e4018d66f 100644
--- a/selfdrive/monitoring/driver_monitor.py
+++ b/selfdrive/monitoring/driver_monitor.py
@@ -1,4 +1,4 @@
-from math import atan2, sqrt
+from math import atan2
 
 from cereal import car
 from common.numpy_fast import interp
@@ -26,21 +26,18 @@ class DRIVER_MONITOR_SETTINGS():
     self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
 
     self._FACE_THRESHOLD = 0.5
-    self._PARTIAL_FACE_THRESHOLD = 0.765 if TICI else 0.455
-    self._EYE_THRESHOLD = 0.25 if TICI else 0.57
-    self._SG_THRESHOLD = 0.83
-    self._BLINK_THRESHOLD = 0.62 if TICI else 0.68
-    self._BLINK_THRESHOLD_SLACK = 0.82 if TICI else 0.88
-    self._BLINK_THRESHOLD_STRICT = 0.62 if TICI else 0.68
-    self._PITCH_WEIGHT = 1.175 if TICI else 1.35  # pitch matters a lot more
-    self._POSESTD_THRESHOLD = 0.2 if TICI else 0.175
-    self._E2E_POSE_THRESHOLD = 0.95 if TICI else 0.9
-    self._E2E_EYES_THRESHOLD = 0.75
+    self._PARTIAL_FACE_THRESHOLD = 0.765 if TICI else 0.43
+    self._EYE_THRESHOLD = 0.61 if TICI else 0.55
+    self._SG_THRESHOLD = 0.89 if TICI else 0.86
+    self._BLINK_THRESHOLD = 0.82 if TICI else 0.588
+    self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.77
+    self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
+    self._PITCH_WEIGHT = 1.35  # pitch matters a lot more
+    self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3
 
-    self._METRIC_THRESHOLD = 0.55 if TICI else 0.48
-    self._METRIC_THRESHOLD_SLACK = 0.75 if TICI else 0.66
-    self._METRIC_THRESHOLD_STRICT = 0.55 if TICI else 0.48
-    self._PITCH_POS_ALLOWANCE = 0.12  # rad, to not be too sensitive on positive pitch
+    self._METRIC_THRESHOLD = 0.48
+    self._METRIC_THRESHOLD_SLACK = 0.66
+    self._METRIC_THRESHOLD_STRICT = self._METRIC_THRESHOLD
     self._PITCH_NATURAL_OFFSET = 0.02  # people don't seem to look straight when they drive relaxed, rather a bit up
     self._YAW_NATURAL_OFFSET = 0.08  # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
 
@@ -170,13 +167,11 @@ class DriverStatus():
       pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
       yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
 
-    # positive pitch allowance
-    if pitch_error > 0.:
-      pitch_error = max(pitch_error - self.settings._PITCH_POS_ALLOWANCE, 0.)
-    pitch_error *= self.settings._PITCH_WEIGHT
-    pose_metric = sqrt(yaw_error**2 + pitch_error**2)
+    pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
+    yaw_error = abs(yaw_error)
 
-    if pose_metric > self.settings._METRIC_THRESHOLD*pose.cfactor:
+    if pitch_error*self.settings._PITCH_WEIGHT > self.settings._METRIC_THRESHOLD*pose.cfactor or \
+       yaw_error > self.settings._METRIC_THRESHOLD*pose.cfactor:
       return DistractedType.BAD_POSE
     elif (blink.left_blink + blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*blink.cfactor:
       return DistractedType.BAD_BLINK
@@ -210,11 +205,8 @@ class DriverStatus():
     self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
     self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
 
-    distracted_normal = self._is_driver_distracted(self.pose, self.blink) > 0 and \
+    self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \
                                    driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
-    distracted_E2E = (driver_state.distractedPose > self.settings._E2E_POSE_THRESHOLD or driver_state.distractedEyes > self.settings._E2E_EYES_THRESHOLD) and \
-                              (self.face_detected and not self.face_partial)
-    self.driver_distracted = distracted_normal or distracted_E2E
     self.driver_distraction_filter.update(self.driver_distracted)
 
     # update offseter
diff --git a/selfdrive/test/helpers.py b/selfdrive/test/helpers.py
index 6a7c1a837..0bb475c92 100644
--- a/selfdrive/test/helpers.py
+++ b/selfdrive/test/helpers.py
@@ -1,10 +1,10 @@
+import os
 import time
 from functools import wraps
-from nose.tools import nottest
 
 from selfdrive.hardware import PC
-from selfdrive.version import training_version, terms_version
 from selfdrive.manager.process_config import managed_processes
+from selfdrive.version import training_version, terms_version
 
 
 def set_params_enabled():
@@ -17,12 +17,21 @@ def set_params_enabled():
   params.put_bool("Passive", False)
 
 
-def phone_only(x):
-  if PC:
-    return nottest(x)
-  else:
-    return x
+def phone_only(f):
+  @wraps(f)
+  def wrap(self, *args, **kwargs):
+    if PC:
+      self.skipTest("This test is not meant to run on PC")
+    f(self, *args, **kwargs)
+  return wrap
 
+def release_only(f):
+  @wraps(f)
+  def wrap(self, *args, **kwargs):
+    if "RELEASE" not in os.environ:
+      self.skipTest("This test is only for release branches")
+    f(self, *args, **kwargs)
+  return wrap
 
 def with_processes(processes, init_time=0, ignore_stopped=None):
   ignore_stopped = [] if ignore_stopped is None else ignore_stopped
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index 342f28c2c..a38ff38fc 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -8,14 +8,16 @@ import unittest
 from collections import Counter
 from pathlib import Path
 
+from cereal import car
 import cereal.messaging as messaging
 from cereal.services import service_list
 from common.basedir import BASEDIR
 from common.timeout import Timeout
 from common.params import Params
+from selfdrive.controls.lib.events import EVENTS, ET
 from selfdrive.hardware import EON, TICI
 from selfdrive.loggerd.config import ROOT
-from selfdrive.test.helpers import set_params_enabled
+from selfdrive.test.helpers import set_params_enabled, release_only
 from tools.lib.logreader import LogReader
 
 # Baseline CPU usage by process
@@ -23,7 +25,7 @@ PROCS = {
   "selfdrive.controls.controlsd": 50.0,
   "./loggerd": 45.0,
   "./locationd": 9.1,
-  "selfdrive.controls.plannerd": 20.0,
+  "selfdrive.controls.plannerd": 22.6,
   "./_ui": 15.0,
   "selfdrive.locationd.paramsd": 9.1,
   "./camerad": 7.07,
@@ -51,17 +53,41 @@ if EON:
 
 if TICI:
   PROCS.update({
-    "./loggerd": 60.0,
+    "./loggerd": 70.0,
     "selfdrive.controls.controlsd": 28.0,
     "./camerad": 31.0,
     "./_ui": 21.0,
-    "selfdrive.controls.plannerd": 12.0,
-    "selfdrive.locationd.paramsd": 5.0,
+    "selfdrive.controls.plannerd": 11.7,
     "./_dmonitoringmodeld": 10.0,
+    "selfdrive.locationd.paramsd": 5.0,
+    "selfdrive.controls.radard": 3.6,
     "selfdrive.thermald.thermald": 1.5,
   })
 
 
+TIMINGS = {
+  # rtols: max/min, rsd
+  "can": [2.5, 0.35],
+  "pandaStates": [2.5, 0.35],
+  "peripheralState": [2.5, 0.35],
+  "sendcan": [2.5, 0.35],
+  "carState": [2.5, 0.35],
+  "carControl": [2.5, 0.35],
+  "controlsState": [2.5, 0.35],
+  "lateralPlan": [2.5, 0.5],
+  "longitudinalPlan": [2.5, 0.5],
+  "roadCameraState": [2.5, 0.35],
+  "driverCameraState": [2.5, 0.35],
+  "modelV2": [2.5, 0.35],
+  "driverState": [2.5, 0.35],
+  "liveLocationKalman": [2.5, 0.35],
+}
+if TICI:
+  TIMINGS.update({
+    "wideRoadCameraState": [1.5, 0.35],
+  })
+
+
 def cputime_total(ct):
   return ct.cpuUser + ct.cpuSystem + ct.cpuChildrenUser + ct.cpuChildrenSystem
 
@@ -102,6 +128,12 @@ class TestOnroad(unittest.TestCase):
 
   @classmethod
   def setUpClass(cls):
+    if "DEBUG" in os.environ:
+      segs = filter(lambda x: os.path.exists(os.path.join(x, "rlog.bz2")), Path(ROOT).iterdir())
+      segs = sorted(segs, key=lambda x: x.stat().st_mtime)
+      cls.lr = list(LogReader(os.path.join(segs[-1], "rlog.bz2")))
+      return
+
     os.environ['SKIP_FW_QUERY'] = "1"
     os.environ['FINGERPRINT'] = "TOYOTA COROLLA TSS2 2019"
     set_params_enabled()
@@ -142,6 +174,9 @@ class TestOnroad(unittest.TestCase):
       if proc.wait(60) is None:
         proc.kill()
 
+    cls.lrs = [list(LogReader(os.path.join(str(s), "rlog.bz2"))) for s in cls.segments]
+
+    # use the second segment by default as it's the first full segment
     cls.lr = list(LogReader(os.path.join(str(cls.segments[1]), "rlog.bz2")))
 
   def test_cloudlog_size(self):
@@ -160,13 +195,50 @@ class TestOnroad(unittest.TestCase):
     cpu_ok = check_cpu_usage(proclogs[0], proclogs[-1])
     self.assertTrue(cpu_ok)
 
-  def test_model_timings(self):
-    #TODO this went up when plannerd cpu usage increased, why?
+  def test_model_execution_timings(self):
+    result =  "------------------------------------------------\n"
+    result += "----------------- Model Timing -----------------\n"
+    result += "------------------------------------------------\n"
+    # TODO: this went up when plannerd cpu usage increased, why?
     cfgs = [("modelV2", 0.038, 0.036), ("driverState", 0.028, 0.026)]
     for (s, instant_max, avg_max) in cfgs:
       ts = [getattr(getattr(m, s), "modelExecutionTime") for m in self.lr if m.which() == s]
       self.assertLess(min(ts), instant_max, f"high '{s}' execution time: {min(ts)}")
       self.assertLess(np.mean(ts), avg_max, f"high avg '{s}' execution time: {np.mean(ts)}")
+      result += f"'{s}' execution time: {min(ts)}\n"
+      result += f"'{s}' avg execution time: {np.mean(ts)}\n"
+    print(result)
+
+  def test_timings(self):
+
+    print("\n\n")
+    print("="*25, "service timings", "="*25)
+    for s, (maxmin, rsd) in TIMINGS.items():
+      msgs = [m.logMonoTime for m in self.lr if m.which() == s]
+      if not len(msgs):
+        raise Exception(f"missing {s}")
+
+      ts = np.diff(msgs) / 1e9
+      dt = 1 / service_list[s].frequency
+
+      np.testing.assert_allclose(np.mean(ts), dt, rtol=0.03, err_msg=f"{s} - failed mean timing check")
+      np.testing.assert_allclose([np.max(ts), np.min(ts)], dt, rtol=maxmin, err_msg=f"{s} - failed max/min timing check")
+      self.assertLess(np.std(ts) / dt, rsd, msg=f"{s} - failed RSD timing check")
+      print(f"{s}: {np.array([np.mean(ts), np.max(ts), np.min(ts)])*1e3}")
+      print(f"     {np.max(np.absolute([np.max(ts)/dt, np.min(ts)/dt]))} {np.std(ts)/dt}")
+    print("="*67)
+
+  @release_only
+  def test_startup(self):
+    startup_alert = None
+    for msg in self.lrs[0]:
+      # can't use carEvents because the first msg can be dropped while loggerd is starting up
+      if msg.which() == "controlsState":
+        startup_alert = msg.controlsState.alertText1
+        break
+    expected = EVENTS[car.CarEvent.EventName.startup][ET.PERMANENT].alert_text_1
+    self.assertEqual(startup_alert, expected, "wrong startup alert")
+
 
 if __name__ == "__main__":
   unittest.main()
diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py
index 70ee9fc0a..9c0736391 100644
--- a/selfdrive/thermald/power_monitoring.py
+++ b/selfdrive/thermald/power_monitoring.py
@@ -28,8 +28,8 @@ class PowerMonitoring:
     self.last_save_time = 0                     # Used for saving current value in a param
     self.power_used_uWh = 0                     # Integrated power usage in uWh since going into offroad
     self.next_pulsed_measurement_time = None
-    self.car_voltage_mV = 12e3                  # Low-passed version of pandaState voltage
-    self.car_voltage_instant_mV = 12e3          # Last value of pandaState voltage
+    self.car_voltage_mV = 12e3                  # Low-passed version of peripheralState voltage
+    self.car_voltage_instant_mV = 12e3          # Last value of peripheralState voltage
     self.integration_lock = threading.Lock()
 
     car_battery_capacity_uWh = self.params.get("CarBatteryCapacity")
@@ -39,14 +39,13 @@ class PowerMonitoring:
     # Reset capacity if it's low
     self.car_battery_capacity_uWh = max((CAR_BATTERY_CAPACITY_uWh / 10), int(car_battery_capacity_uWh))
 
-
   # Calculation tick
-  def calculate(self, pandaState):
+  def calculate(self, peripheralState, ignition):
     try:
       now = sec_since_boot()
 
-      # If pandaState is None, we're probably not in a car, so we don't care
-      if pandaState is None or pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
+      # If peripheralState is None, we're probably not in a car, so we don't care
+      if peripheralState is None or peripheralState.pandaType == log.PandaState.PandaType.unknown:
         with self.integration_lock:
           self.last_measurement_time = None
           self.next_pulsed_measurement_time = None
@@ -54,8 +53,8 @@ class PowerMonitoring:
         return
 
       # Low-pass battery voltage
-      self.car_voltage_instant_mV = pandaState.pandaState.voltage
-      self.car_voltage_mV = ((pandaState.pandaState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 -  CAR_VOLTAGE_LOW_PASS_K)))
+      self.car_voltage_instant_mV = peripheralState.voltage
+      self.car_voltage_mV = ((peripheralState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
 
       # Cap the car battery power and save it in a param every 10-ish seconds
       self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0)
@@ -70,7 +69,7 @@ class PowerMonitoring:
           self.last_measurement_time = now
           return
 
-      if (pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan):
+      if ignition:
         # If there is ignition, we integrate the charging rate of the car
         with self.integration_lock:
           self.power_used_uWh = 0
@@ -81,7 +80,7 @@ class PowerMonitoring:
           self.last_measurement_time = now
       else:
         # No ignition, we integrate the offroad power used by the device
-        is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
+        is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
         # Get current power draw somehow
         current_power = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
         if current_power is not None:
@@ -158,8 +157,8 @@ class PowerMonitoring:
     return int(self.car_battery_capacity_uWh)
 
   # See if we need to disable charging
-  def should_disable_charging(self, pandaState, offroad_timestamp):
-    if pandaState is None or offroad_timestamp is None:
+  def should_disable_charging(self, ignition, in_car, offroad_timestamp):
+    if offroad_timestamp is None:
       return False
 
     now = sec_since_boot()
@@ -167,24 +166,24 @@ class PowerMonitoring:
     disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S
     disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3)) and (self.car_voltage_instant_mV > (VBATT_INSTANT_PAUSE_CHARGING * 1e3))
     disable_charging |= (self.car_battery_capacity_uWh <= 0)
-    disable_charging &= (not pandaState.pandaState.ignitionLine and not pandaState.pandaState.ignitionCan)
+    disable_charging &= not ignition
     disable_charging &= (not self.params.get_bool("DisablePowerDown"))
-    disable_charging &= (pandaState.pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected)
+    disable_charging &= in_car
     disable_charging |= self.params.get_bool("ForcePowerDown")
     return disable_charging
 
   # See if we need to shutdown
-  def should_shutdown(self, pandaState, offroad_timestamp, started_seen):
-    if pandaState is None or offroad_timestamp is None:
+  def should_shutdown(self, peripheralState, ignition, in_car, offroad_timestamp, started_seen):
+    if offroad_timestamp is None:
       return False
 
     now = sec_since_boot()
-    panda_charging = (pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client)
+    panda_charging = (peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client)
     BATT_PERC_OFF = 10
 
     should_shutdown = False
     # Wait until we have shut down charging before powering down
-    should_shutdown |= (not panda_charging and self.should_disable_charging(pandaState, offroad_timestamp))
+    should_shutdown |= (not panda_charging and self.should_disable_charging(ignition, in_car, offroad_timestamp))
     should_shutdown |= ((HARDWARE.get_battery_capacity() < BATT_PERC_OFF) and (not HARDWARE.get_battery_charging()) and ((now - offroad_timestamp) > 60))
     should_shutdown &= started_seen or (now > MIN_ON_TIME_S)
     return should_shutdown
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index 67cdf7030..68034b681 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -32,8 +32,8 @@ NetworkType = log.DeviceState.NetworkType
 NetworkStrength = log.DeviceState.NetworkStrength
 CURRENT_TAU = 15.   # 15s time constant
 TEMP_TAU = 5.   # 5s time constant
-DAYS_NO_CONNECTIVITY_MAX = 7  # do not allow to engage after a week without internet
-DAYS_NO_CONNECTIVITY_PROMPT = 4  # send an offroad prompt after 4 days with no internet
+DAYS_NO_CONNECTIVITY_MAX = 14     # do not allow to engage after this many days
+DAYS_NO_CONNECTIVITY_PROMPT = 10  # send an offroad prompt after this many days
 DISCONNECT_TIMEOUT = 5.  # wait 5 seconds before going offroad after disconnect so you get an alert
 
 ThermalBand = namedtuple("ThermalBand", ['min_temp', 'max_temp'])
@@ -48,7 +48,7 @@ THERMAL_BANDS = OrderedDict({
 })
 
 # Override to highest thermal band when offroad and above this temp
-OFFROAD_DANGER_TEMP = 70.0
+OFFROAD_DANGER_TEMP = 79.5 if TICI else 70.0
 
 prev_offroad_states: Dict[str, Tuple[bool, Optional[str]]] = {}
 
@@ -154,9 +154,8 @@ def thermald_thread():
   pm = messaging.PubMaster(['deviceState'])
 
   pandaState_timeout = int(1000 * 2.5 * DT_TRML)  # 2.5x the expected pandaState frequency
-  pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout)
-  location_sock = messaging.sub_sock('gpsLocationExternal')
-  managerState_sock = messaging.sub_sock('managerState', conflate=True)
+  pandaState_sock = messaging.sub_sock('pandaStates', timeout=pandaState_timeout)
+  sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "managerState"])
 
   fan_speed = 0
   count = 0
@@ -177,11 +176,14 @@ def thermald_thread():
   network_info = None
   modem_version = None
   registered_count = 0
+  nvme_temps = None
+  modem_temps = None
 
   current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
   temp_filter = FirstOrderFilter(0., TEMP_TAU, DT_TRML)
   pandaState_prev = None
   should_start_prev = False
+  in_car = False
   handle_fan = None
   is_uno = False
   ui_running_prev = False
@@ -196,32 +198,23 @@ def thermald_thread():
   # TODO: use PI controller for UNO
   controller = PIController(k_p=0, k_i=2e-3, neg_limit=-80, pos_limit=0, rate=(1 / DT_TRML))
 
+  # Leave flag for loggerd to indicate device was left onroad
   if params.get_bool("IsOnroad"):
-    cloudlog.event("onroad flag not cleared")
+    params.put_bool("BootedOnroad", True)
 
-  # CPR3 logging
-  if EON:
-    base_path = "/sys/kernel/debug/cpr3-regulator/"
-    cpr_files = [p for p in Path(base_path).glob("**/*") if p.is_file()]
-    cpr_files = ["/sys/kernel/debug/regulator/pm8994_s11/voltage"] + cpr_files
-    cpr_data = {}
-    for cf in cpr_files:
-      with open(cf, "r") as f:
-        try:
-          cpr_data[str(cf)] = f.read().strip()
-        except Exception:
-          pass
-    cloudlog.event("CPR", data=cpr_data)
+  while True:
+    pandaStates = messaging.recv_sock(pandaState_sock, wait=True)
+
+    sm.update(0)
+    peripheralState = sm['peripheralState']
 
-  while 1:
-    pandaState = messaging.recv_sock(pandaState_sock, wait=True)
     msg = read_thermal(thermal_config)
 
-    if pandaState is not None:
-      usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
+    if pandaStates is not None and len(pandaStates.pandaStates) > 0:
+      pandaState = pandaStates.pandaStates[0]
 
       # If we lose connection to the panda, wait 5 seconds before going offroad
-      if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
+      if pandaState.pandaType == log.PandaState.PandaType.unknown:
         no_panda_cnt += 1
         if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
           if startup_conditions["ignition"]:
@@ -229,15 +222,14 @@ def thermald_thread():
           startup_conditions["ignition"] = False
       else:
         no_panda_cnt = 0
-        startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
+        startup_conditions["ignition"] = pandaState.ignitionLine or pandaState.ignitionCan
 
-      startup_conditions["hardware_supported"] = pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda,
-                                                                                         log.PandaState.PandaType.greyPanda]
-      set_offroad_alert_if_changed("Offroad_HardwareUnsupported", not startup_conditions["hardware_supported"])
+      in_car = pandaState.harnessStatus != log.PandaState.HarnessStatus.notConnected
+      usb_power = peripheralState.usbPowerMode != log.PeripheralState.UsbPowerMode.client
 
       # Setup fan handler on first connect to panda
-      if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
-        is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
+      if handle_fan is None and peripheralState.pandaType != log.PandaState.PandaType.unknown:
+        is_uno = peripheralState.pandaType == log.PandaState.PandaType.uno
 
         if TICI:
           cloudlog.info("Setting up TICI fan handler")
@@ -252,17 +244,19 @@ def thermald_thread():
 
       # Handle disconnect
       if pandaState_prev is not None:
-        if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
-          pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
+        if pandaState.pandaType == log.PandaState.PandaType.unknown and \
+          pandaState_prev.pandaType != log.PandaState.PandaType.unknown:
           params.clear_all(ParamKeyType.CLEAR_ON_PANDA_DISCONNECT)
       pandaState_prev = pandaState
 
-    # get_network_type is an expensive call. update every 10s
+    # these are expensive calls. update every 10s
     if (count % int(10. / DT_TRML)) == 0:
       try:
         network_type = HARDWARE.get_network_type()
         network_strength = HARDWARE.get_network_strength(network_type)
         network_info = HARDWARE.get_network_info()  # pylint: disable=assignment-from-none
+        nvme_temps = HARDWARE.get_nvme_temperatures()
+        modem_temps = HARDWARE.get_modem_temperatures()
 
         # Log modem version once
         if modem_version is None:
@@ -291,7 +285,12 @@ def thermald_thread():
     msg.deviceState.networkStrength = network_strength
     if network_info is not None:
       msg.deviceState.networkInfo = network_info
+    if nvme_temps is not None:
+      msg.deviceState.nvmeTempC = nvme_temps
+    if modem_temps is not None:
+      msg.deviceState.modemTempC = modem_temps
 
+    msg.deviceState.screenBrightnessPercent = HARDWARE.get_screen_brightness()
     msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
     msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
     msg.deviceState.usbOnline = HARDWARE.get_usb_present()
@@ -404,27 +403,25 @@ def thermald_thread():
         off_ts = sec_since_boot()
 
     # Offroad power monitoring
-    power_monitor.calculate(pandaState)
+    power_monitor.calculate(peripheralState, startup_conditions["ignition"])
     msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
     msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
 
     # Check if we need to disable charging (handled by boardd)
-    msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts)
+    msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts)
 
     # Check if we need to shut down
-    if power_monitor.should_shutdown(pandaState, off_ts, started_seen):
+    if power_monitor.should_shutdown(peripheralState, startup_conditions["ignition"], in_car, off_ts, started_seen):
       cloudlog.info(f"shutting device down, offroad since {off_ts}")
       # TODO: add function for blocking cloudlog instead of sleep
       time.sleep(10)
       HARDWARE.shutdown()
 
     # If UI has crashed, set the brightness to reasonable non-zero value
-    manager_state = messaging.recv_one_or_none(managerState_sock)
-    if manager_state is not None:
-      ui_running = "ui" in (p.name for p in manager_state.managerState.processes if p.running)
-      if ui_running_prev and not ui_running:
-        HARDWARE.set_screen_brightness(20)
-      ui_running_prev = ui_running
+    ui_running = "ui" in (p.name for p in sm["managerState"].processes if p.running)
+    if ui_running_prev and not ui_running:
+      HARDWARE.set_screen_brightness(20)
+    ui_running_prev = ui_running
 
     msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90  # if current is positive, then battery is being discharged
     msg.deviceState.started = started_ts is not None
@@ -448,11 +445,11 @@ def thermald_thread():
       if EON and started_ts is None and msg.deviceState.memoryUsagePercent > 40:
         cloudlog.event("High offroad memory usage", mem=msg.deviceState.memoryUsagePercent)
 
-      location = messaging.recv_sock(location_sock)
       cloudlog.event("STATUS_PACKET",
                      count=count,
-                     pandaState=(strip_deprecated_keys(pandaState.to_dict()) if pandaState else None),
-                     location=(strip_deprecated_keys(location.gpsLocationExternal.to_dict()) if location else None),
+                     pandaStates=(strip_deprecated_keys(pandaStates.to_dict()) if pandaStates else None),
+                     peripheralState=strip_deprecated_keys(peripheralState.to_dict()),
+                     location=(strip_deprecated_keys(sm["gpsLocationExternal"].to_dict()) if sm.alive["gpsLocationExternal"] else None),
                      deviceState=strip_deprecated_keys(msg.to_dict()))
 
     count += 1
diff --git a/selfdrive/ui/.gitignore b/selfdrive/ui/.gitignore
index 8646fed18..9e529b5d0 100644
--- a/selfdrive/ui/.gitignore
+++ b/selfdrive/ui/.gitignore
@@ -1,9 +1,11 @@
 moc_*
 *.moc
 
+watch3
 installer/installers/*
-
+tests/playsound
 replay/replay
+replay/tests/test_replay
 qt/text
 qt/spinner
 qt/setup/setup
diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index 6ec37a232..3a1e40668 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -11,7 +11,7 @@ if arch == 'aarch64':
   base_libs += ['log', 'utils', 'gui', 'ui', 'CB', 'gsl', 'adreno_utils', 'cutils', 'uuid']
 
 if maps and arch == 'x86_64':
-  rpath = [Dir(f"#phonelibs/mapbox-gl-native-qt/{arch}").srcnode().abspath]
+  rpath = [Dir(f"#third_party/mapbox-gl-native-qt/{arch}").srcnode().abspath]
   qt_env["RPATH"] += rpath
 
 if arch == "Darwin":
@@ -21,7 +21,7 @@ if arch == "Darwin":
 widgets_src = ["qt/util.cc", "qt/widgets/input.cc", "qt/widgets/drive_stats.cc",
                "qt/widgets/ssh_keys.cc", "qt/widgets/toggle.cc", "qt/widgets/controls.cc",
                "qt/widgets/offroad_alerts.cc", "qt/widgets/prime.cc", "qt/widgets/keyboard.cc",
-               "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#phonelibs/qrcode/QrCode.cc", "qt/api.cc",
+               "qt/widgets/scrollview.cc", "qt/widgets/cameraview.cc", "#third_party/qrcode/QrCode.cc", "qt/api.cc",
                "qt/request_repeater.cc"]
 
 if arch != 'aarch64':
@@ -45,6 +45,8 @@ asset_obj = qt_env.Object("assets", assets)
 
 # build soundd
 qt_env.Program("_soundd", "soundd.cc", LIBS=base_libs)
+if GetOption('test'):
+  qt_env.Program("tests/playsound", "tests/playsound.cc", LIBS=base_libs)
 
 # spinner and text window
 qt_env.Program("qt/text", ["qt/text.cc"], LIBS=qt_libs)
@@ -54,21 +56,22 @@ qt_env.Program("qt/spinner", ["qt/spinner.cc"], LIBS=qt_libs)
 qt_src = ["main.cc", "ui.cc", "paint.cc", "qt/sidebar.cc", "qt/onroad.cc",
           "qt/window.cc", "qt/home.cc", "qt/offroad/settings.cc",
           "qt/offroad/onboarding.cc", "qt/offroad/driverview.cc",
-          "#phonelibs/nanovg/nanovg.c"]
+          "#third_party/nanovg/nanovg.c"]
 qt_env.Program("_ui", qt_src + [asset_obj], LIBS=qt_libs)
 
 
-# setup, factory resetter, and agnos updater
+# setup and factory resetter
 if arch != 'aarch64' and GetOption('setup'):
-
   qt_env.Program("qt/setup/reset", ["qt/setup/reset.cc"], LIBS=qt_libs)
-  qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs)
   qt_env.Program("qt/setup/setup", ["qt/setup/setup.cc", asset_obj],
                  LIBS=qt_libs + ['curl', 'common', 'json11'])
 
 
-# build installers
 if GetOption('setup'):
+  # buidl updater UI
+  qt_env.Program("qt/setup/updater", ["qt/setup/updater.cc", asset_obj], LIBS=qt_libs)
+
+  # build installers
   senv = qt_env.Clone()
   senv['LINKFLAGS'].append('-Wl,-strip-debug')
 
@@ -107,8 +110,13 @@ if GetOption('setup'):
 if arch in ['x86_64', 'Darwin'] and os.path.exists(Dir("#tools/").get_abspath()):
   qt_env['CXXFLAGS'] += ["-Wno-deprecated-declarations"]
 
-  replay_lib_src = ["replay/replay.cc", "replay/filereader.cc", "replay/framereader.cc"]
+  replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"]
 
   replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs)
-  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'swscale', 'bz2'] + qt_libs
+  replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs
   qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs)
+
+  qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11'])
+
+  if GetOption('test'):
+    qt_env.Program('replay/tests/test_replay', ['replay/tests/test_runner.cc', 'replay/tests/test_replay.cc'], LIBS=[replay_libs])
diff --git a/selfdrive/ui/paint.cc b/selfdrive/ui/paint.cc
index 40972e07a..363be71a5 100644
--- a/selfdrive/ui/paint.cc
+++ b/selfdrive/ui/paint.cc
@@ -1,6 +1,5 @@
 #include "selfdrive/ui/paint.h"
 
-#include <algorithm>
 #include <cassert>
 
 #ifdef __APPLE__
@@ -17,10 +16,8 @@
 #include <nanovg_gl.h>
 #include <nanovg_gl_utils.h>
 
-#include "selfdrive/common/timing.h"
 #include "selfdrive/common/util.h"
 #include "selfdrive/hardware/hw.h"
-
 #include "selfdrive/ui/ui.h"
 
 static void ui_draw_text(const UIState *s, float x, float y, const char *string, float size, NVGcolor color, const char *font_name) {
@@ -108,31 +105,6 @@ static void ui_draw_line(UIState *s, const line_vertices_data &vd, NVGcolor *col
   nvgFill(s->vg);
 }
 
-static void draw_vision_frame(UIState *s) {
-  glBindVertexArray(s->frame_vao);
-  mat4 *out_mat = &s->rear_frame_mat;
-  glActiveTexture(GL_TEXTURE0);
-
-  if (s->last_frame) {
-    glBindTexture(GL_TEXTURE_2D, s->texture[s->last_frame->idx]->frame_tex);
-    if (!Hardware::EON()) {
-      // this is handled in ion on QCOM
-      glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, s->last_frame->width, s->last_frame->height,
-                   0, GL_RGB, GL_UNSIGNED_BYTE, s->last_frame->addr);
-    }
-  }
-
-  glUseProgram(s->gl_shader->prog);
-  glUniform1i(s->gl_shader->getUniformLocation("uTexture"), 0);
-  glUniformMatrix4fv(s->gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, out_mat->v);
-
-  assert(glGetError() == GL_NO_ERROR);
-  glEnableVertexAttribArray(0);
-  glDrawElements(GL_TRIANGLES, 6, GL_UNSIGNED_BYTE, (const void *)0);
-  glDisableVertexAttribArray(0);
-  glBindVertexArray(0);
-}
-
 static void ui_draw_vision_lane_lines(UIState *s) {
   const UIScene &scene = s->scene;
   NVGpaint track_bg;
@@ -172,7 +144,7 @@ static void ui_draw_world(UIState *s) {
     if (lead_one.getProb() > .5) {
       draw_lead(s, lead_one, s->scene.lead_vertices[0]);
     }
-   if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
+    if (lead_two.getProb() > .5 && (std::abs(lead_one.getX()[0] - lead_two.getX()[0]) > 3.0)) {
       draw_lead(s, lead_two, s->scene.lead_vertices[1]);
     }
   }
@@ -249,19 +221,14 @@ static void ui_draw_vision(UIState *s) {
 }
 
 void ui_draw(UIState *s, int w, int h) {
-  const bool draw_vision = s->scene.started && s->vipc_client->connected;
-
-  glViewport(0, 0, s->fb_w, s->fb_h);
-  if (draw_vision) {
-    draw_vision_frame(s);
+  // Update intrinsics matrix after possible wide camera toggle change
+  if (s->fb_w != w || s->fb_h != h) {
+    ui_resize(s, w, h);
   }
   glEnable(GL_BLEND);
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
-  // NVG drawing functions - should be no GL inside NVG frame
   nvgBeginFrame(s->vg, s->fb_w, s->fb_h, 1.0f);
-  if (draw_vision) {
-    ui_draw_vision(s);
-  }
+  ui_draw_vision(s);
   nvgEndFrame(s->vg);
   glDisable(GL_BLEND);
 }
@@ -296,49 +263,7 @@ void ui_fill_rect(NVGcontext *vg, const Rect &r, const NVGpaint &paint, float ra
   fill_rect(vg, r, nullptr, &paint, radius);
 }
 
-static const char frame_vertex_shader[] =
-#ifdef NANOVG_GL3_IMPLEMENTATION
-  "#version 150 core\n"
-#else
-  "#version 300 es\n"
-#endif
-  "in vec4 aPosition;\n"
-  "in vec4 aTexCoord;\n"
-  "uniform mat4 uTransform;\n"
-  "out vec4 vTexCoord;\n"
-  "void main() {\n"
-  "  gl_Position = uTransform * aPosition;\n"
-  "  vTexCoord = aTexCoord;\n"
-  "}\n";
-
-static const char frame_fragment_shader[] =
-#ifdef NANOVG_GL3_IMPLEMENTATION
-  "#version 150 core\n"
-#else
-  "#version 300 es\n"
-#endif
-  "precision mediump float;\n"
-  "uniform sampler2D uTexture;\n"
-  "in vec4 vTexCoord;\n"
-  "out vec4 colorOut;\n"
-  "void main() {\n"
-  "  colorOut = texture(uTexture, vTexCoord.xy);\n"
-#ifdef QCOM
-  "  vec3 dz = vec3(0.0627f, 0.0627f, 0.0627f);\n"
-  "  colorOut.rgb = ((vec3(1.0f, 1.0f, 1.0f) - dz) * colorOut.rgb / vec3(1.0f, 1.0f, 1.0f)) + dz;\n"
-#endif
-  "}\n";
-
-static const mat4 device_transform = {{
-  1.0,  0.0, 0.0, 0.0,
-  0.0,  1.0, 0.0, 0.0,
-  0.0,  0.0, 1.0, 0.0,
-  0.0,  0.0, 0.0, 1.0,
-}};
-
 void ui_nvg_init(UIState *s) {
-  // init drawing
-
   // on EON, we enable MSAA
   s->vg = Hardware::EON() ? nvgCreate(0) : nvgCreate(NVG_ANTIALIAS | NVG_STENCIL_STROKES | NVG_DEBUG);
   assert(s->vg);
@@ -363,45 +288,6 @@ void ui_nvg_init(UIState *s) {
     s->images[name] = nvgCreateImage(s->vg, file, 1);
     assert(s->images[name] != 0);
   }
-
-  // init gl
-  s->gl_shader = std::make_unique<GLShader>(frame_vertex_shader, frame_fragment_shader);
-  GLint frame_pos_loc = glGetAttribLocation(s->gl_shader->prog, "aPosition");
-  GLint frame_texcoord_loc = glGetAttribLocation(s->gl_shader->prog, "aTexCoord");
-
-  glViewport(0, 0, s->fb_w, s->fb_h);
-
-  glDisable(GL_DEPTH_TEST);
-
-  assert(glGetError() == GL_NO_ERROR);
-
-  float x1 = 1.0, x2 = 0.0, y1 = 1.0, y2 = 0.0;
-  const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
-  const float frame_coords[4][4] = {
-    {-1.0, -1.0, x2, y1}, //bl
-    {-1.0,  1.0, x2, y2}, //tl
-    { 1.0,  1.0, x1, y2}, //tr
-    { 1.0, -1.0, x1, y1}, //br
-  };
-
-  glGenVertexArrays(1, &s->frame_vao);
-  glBindVertexArray(s->frame_vao);
-  glGenBuffers(1, &s->frame_vbo);
-  glBindBuffer(GL_ARRAY_BUFFER, s->frame_vbo);
-  glBufferData(GL_ARRAY_BUFFER, sizeof(frame_coords), frame_coords, GL_STATIC_DRAW);
-  glEnableVertexAttribArray(frame_pos_loc);
-  glVertexAttribPointer(frame_pos_loc, 2, GL_FLOAT, GL_FALSE,
-                        sizeof(frame_coords[0]), (const void *)0);
-  glEnableVertexAttribArray(frame_texcoord_loc);
-  glVertexAttribPointer(frame_texcoord_loc, 2, GL_FLOAT, GL_FALSE,
-                        sizeof(frame_coords[0]), (const void *)(sizeof(float) * 2));
-  glGenBuffers(1, &s->frame_ibo);
-  glBindBuffer(GL_ELEMENT_ARRAY_BUFFER, s->frame_ibo);
-  glBufferData(GL_ELEMENT_ARRAY_BUFFER, sizeof(frame_indicies), frame_indicies, GL_STATIC_DRAW);
-  glBindBuffer(GL_ARRAY_BUFFER, 0);
-  glBindVertexArray(0);
-
-  ui_resize(s, s->fb_w, s->fb_h);
 }
 
 void ui_resize(UIState *s, int width, int height) {
@@ -409,25 +295,11 @@ void ui_resize(UIState *s, int width, int height) {
   s->fb_h = height;
 
   auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
-
   float zoom = ZOOM / intrinsic_matrix.v[0];
-
   if (s->wide_camera) {
     zoom *= 0.5;
   }
 
-  float zx = zoom * 2 * intrinsic_matrix.v[2] / width;
-  float zy = zoom * 2 * intrinsic_matrix.v[5] / height;
-
-  const mat4 frame_transform = {{
-    zx, 0.0, 0.0, 0.0,
-    0.0, zy, 0.0, -y_offset / height * 2,
-    0.0, 0.0, 1.0, 0.0,
-    0.0, 0.0, 0.0, 1.0,
-  }};
-
-  s->rear_frame_mat = matmul(device_transform, frame_transform);
-
   // Apply transformation such that video pixel coordinates match video
   // 1) Put (0, 0) in the middle of the video
   nvgTranslate(s->vg, width / 2, height / 2 + y_offset);
diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc
index 90ba5fd53..0c344056f 100644
--- a/selfdrive/ui/qt/api.cc
+++ b/selfdrive/ui/qt/api.cc
@@ -92,7 +92,10 @@ void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Meth
 
   QNetworkRequest request;
   request.setUrl(QUrl(requestURL));
-  request.setRawHeader(QByteArray("Authorization"), ("JWT " + token).toUtf8());
+
+  if (!token.isEmpty()) {
+    request.setRawHeader(QByteArray("Authorization"), ("JWT " + token).toUtf8());
+  }
 
   if (method == HttpRequest::Method::GET) {
     reply = networkAccessManager->get(request);
@@ -119,8 +122,11 @@ void HttpRequest::requestFinished() {
       success = true;
       emit receivedResponse(response);
     } else {
-      qDebug() << reply->errorString();
       emit failedResponse(reply->errorString());
+
+      if (reply->error() == QNetworkReply::ContentAccessDenied || reply->error() == QNetworkReply::AuthenticationRequiredError) {
+        qWarning() << ">>  Unauthorized. Authenticate with tools/lib/auth.py  <<";
+      }
     }
   } else {
     networkAccessManager->clearAccessCache();
diff --git a/selfdrive/ui/qt/maps/map.cc b/selfdrive/ui/qt/maps/map.cc
index 24f529a7e..e07bf58be 100644
--- a/selfdrive/ui/qt/maps/map.cc
+++ b/selfdrive/ui/qt/maps/map.cc
@@ -3,6 +3,7 @@
 #include <cmath>
 
 #include <QDebug>
+#include <QPainterPath>
 
 #include "selfdrive/common/swaglog.h"
 #include "selfdrive/ui/ui.h"
diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc
index 770b01902..9df155d62 100644
--- a/selfdrive/ui/qt/maps/map_settings.cc
+++ b/selfdrive/ui/qt/maps/map_settings.cc
@@ -4,7 +4,6 @@
 
 #include "selfdrive/common/util.h"
 #include "selfdrive/ui/qt/util.h"
-#include "selfdrive/ui/qt/qt_window.h"
 #include "selfdrive/ui/qt/request_repeater.h"
 #include "selfdrive/ui/qt/widgets/controls.h"
 #include "selfdrive/ui/qt/widgets/scrollview.h"
@@ -102,7 +101,7 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
 
     QLabel *screenshot = new QLabel;
     QPixmap pm = QPixmap("../assets/navigation/screenshot.png");
-    screenshot->setPixmap(pm.scaledToWidth(vwp_w * 0.5, Qt::SmoothTransformation));
+    screenshot->setPixmap(pm.scaledToWidth(1080, Qt::SmoothTransformation));
     no_prime_layout->addWidget(screenshot, 0, Qt::AlignHCenter);
 
     QLabel *signup = new QLabel("Get turn-by-turn directions displayed and more with a comma \nprime subscription. Sign up now: https://connect.comma.ai");
diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc
index af9750c23..db2a4d6b4 100644
--- a/selfdrive/ui/qt/offroad/driverview.cc
+++ b/selfdrive/ui/qt/offroad/driverview.cc
@@ -66,22 +66,27 @@ void DriverViewScene::paintEvent(QPaintEvent* event) {
   const QRect& blackout_rect = Hardware::TICI() ? rect() : rect2;
   p.fillRect(blackout_rect.adjusted(0, 0, valid_rect.left() - blackout_rect.right(), 0), bg);
   p.fillRect(blackout_rect.adjusted(valid_rect.right() - blackout_rect.left(), 0, 0, 0), bg);
+  if (Hardware::TICI()) {
+    p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz
+  }
 
   // face bounding box
   cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState();
-  bool face_detected = driver_state.getFaceProb() > 0.4;
+  bool face_detected = driver_state.getFaceProb() > 0.5;
   if (face_detected) {
     auto fxy_list = driver_state.getFacePosition();
+    auto std_list = driver_state.getFaceOrientationStd();
     float face_x = fxy_list[0];
     float face_y = fxy_list[1];
+    float face_std = std::max(std_list[0], std_list[1]);
 
-    float alpha = 0.2;
-    float x = std::abs(face_x), y = std::abs(face_y);
-    if (x <= 0.35 && y <= 0.4) {
-      alpha = 0.8 - std::max(x, y) * 0.6 / 0.375;
+    float alpha = 0.7;
+    if (face_std > 0.08) {
+      alpha = std::max(0.7 - (face_std-0.08)*7, 0.0);
     }
     const int box_size = 0.6 * rect2.height() / 2;
-    int fbox_x = valid_rect.center().x() + (is_rhd ? face_x : -face_x) * valid_rect.width();
+    const float rhd_offset = 0.05; // lhd is shifted, so rhd is not mirrored
+    int fbox_x = valid_rect.center().x() + (is_rhd ? (face_x + rhd_offset) : -face_x) * valid_rect.width();
     int fbox_y = valid_rect.center().y() + face_y * valid_rect.height();
     p.setPen(QPen(QColor(255, 255, 255, alpha * 255), 10));
     p.drawRoundedRect(fbox_x - box_size / 2, fbox_y - box_size / 2, box_size, box_size, 35.0, 35.0);
diff --git a/selfdrive/ui/qt/offroad/networking.cc b/selfdrive/ui/qt/offroad/networking.cc
index e317b25b9..05ca31225 100644
--- a/selfdrive/ui/qt/offroad/networking.cc
+++ b/selfdrive/ui/qt/offroad/networking.cc
@@ -10,6 +10,7 @@
 
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/qt_window.h"
+#include "selfdrive/ui/qt/widgets/controls.h"
 #include "selfdrive/ui/qt/widgets/scrollview.h"
 
 
@@ -123,11 +124,11 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
   connect(back, &QPushButton::clicked, [=]() { emit backPress(); });
   main_layout->addWidget(back, 0, Qt::AlignLeft);
 
+  ListWidget *list = new ListWidget(this);
   // Enable tethering layout
   tetheringToggle = new ToggleControl("Enable Tethering", "", "", wifi->isTetheringEnabled());
-  main_layout->addWidget(tetheringToggle);
+  list->addItem(tetheringToggle);
   QObject::connect(tetheringToggle, &ToggleControl::toggleFlipped, this, &AdvancedNetworking::toggleTethering);
-  main_layout->addWidget(horizontal_line(), 0);
 
   // Change tethering password
   ButtonControl *editPasswordButton = new ButtonControl("Tethering Password", "EDIT");
@@ -137,30 +138,45 @@ AdvancedNetworking::AdvancedNetworking(QWidget* parent, WifiManager* wifi): QWid
       wifi->changeTetheringPassword(pass);
     }
   });
-  main_layout->addWidget(editPasswordButton, 0);
-  main_layout->addWidget(horizontal_line(), 0);
+  list->addItem(editPasswordButton);
 
   // IP address
   ipLabel = new LabelControl("IP Address", wifi->ipv4_address);
-  main_layout->addWidget(ipLabel, 0);
-  main_layout->addWidget(horizontal_line(), 0);
+  list->addItem(ipLabel);
 
   // SSH keys
-  main_layout->addWidget(new SshToggle());
-  main_layout->addWidget(horizontal_line(), 0);
-  main_layout->addWidget(new SshControl());
-  main_layout->addWidget(horizontal_line(), 0);
+  list->addItem(new SshToggle());
+  list->addItem(new SshControl());
 
   // Roaming toggle
   const bool roamingEnabled = params.getBool("GsmRoaming");
-  wifi->setRoamingEnabled(roamingEnabled);
   ToggleControl *roamingToggle = new ToggleControl("Enable Roaming", "", "", roamingEnabled);
   QObject::connect(roamingToggle, &SshToggle::toggleFlipped, [=](bool state) {
     params.putBool("GsmRoaming", state);
-    wifi->setRoamingEnabled(state);
+    wifi->updateGsmSettings(state, QString::fromStdString(params.get("GsmApn")));
   });
-  main_layout->addWidget(roamingToggle);
+  list->addItem(roamingToggle);
 
+  // APN settings
+  ButtonControl *editApnButton = new ButtonControl("APN settings", "EDIT");
+  connect(editApnButton, &ButtonControl::clicked, [=]() {
+    const bool roamingEnabled = params.getBool("GsmRoaming");
+    const QString cur_apn = QString::fromStdString(params.get("GsmApn"));
+    QString apn = InputDialog::getText("Enter APN", this, "leave blank for automatic configuration", false, -1, cur_apn).trimmed();
+
+    if (apn.isEmpty()) {
+      params.remove("GsmApn");
+    } else {
+      params.put("GsmApn", apn.toStdString());
+    }
+    wifi->updateGsmSettings(roamingEnabled, apn);
+  });
+  list->addItem(editApnButton);
+
+  // Set initial config
+  wifi->updateGsmSettings(roamingEnabled,  QString::fromStdString(params.get("GsmApn")));
+
+  main_layout->addWidget(new ScrollView(list, this));
   main_layout->addStretch(1);
 }
 
@@ -253,7 +269,7 @@ void WifiUI::refresh() {
   std::sort(sortedNetworks.begin(), sortedNetworks.end(), compare_by_strength);
 
   // add networks
-  int i = 0;
+  ListWidget *list = new ListWidget(this);
   for (Network &network : sortedNetworks) {
     QHBoxLayout *hlayout = new QHBoxLayout;
     hlayout->setContentsMargins(44, 0, 73, 0);
@@ -280,7 +296,7 @@ void WifiUI::refresh() {
       QPushButton *forgetBtn = new QPushButton("FORGET");
       forgetBtn->setObjectName("forgetBtn");
       QObject::connect(forgetBtn, &QPushButton::clicked, [=]() {
-        if (ConfirmationDialog::confirm("Forget WiFi Network \"" + QString::fromUtf8(network.ssid) + "\"?", this)) {
+        if (ConfirmationDialog::confirm("Forget Wi-Fi Network \"" + QString::fromUtf8(network.ssid) + "\"?", this)) {
           wifi->forgetConnection(network.ssid);
         }
       });
@@ -309,13 +325,8 @@ void WifiUI::refresh() {
     strength->setPixmap(strengths[std::clamp((int)round(network.strength / 33.), 0, 3)]);
     hlayout->addWidget(strength, 0, Qt::AlignRight);
 
-    main_layout->addLayout(hlayout);
-
-    // Don't add the last horizontal line
-    if (i+1 < wifi->seenNetworks.size()) {
-      main_layout->addWidget(horizontal_line(), 0);
-    }
-    i++;
+    list->addItem(hlayout);
   }
+  main_layout->addWidget(list);
   main_layout->addStretch(1);
 }
diff --git a/selfdrive/ui/qt/offroad/networkmanager.h b/selfdrive/ui/qt/offroad/networkmanager.h
index d90371b60..52d85c16a 100644
--- a/selfdrive/ui/qt/offroad/networkmanager.h
+++ b/selfdrive/ui/qt/offroad/networkmanager.h
@@ -33,5 +33,6 @@ const QString NM_DBUS_SERVICE                        = "org.freedesktop.NetworkM
 const int NM_DEVICE_STATE_ACTIVATED = 100;
 const int NM_DEVICE_STATE_NEED_AUTH = 60;
 const int NM_DEVICE_TYPE_WIFI = 2;
+const int NM_DEVICE_TYPE_MODEM = 8;
 const int NM_DEVICE_STATE_REASON_SUPPLICANT_DISCONNECT = 8;
 const int DBUS_TIMEOUT = 100;
diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc
index e05b345f7..aa03ac63d 100644
--- a/selfdrive/ui/qt/offroad/onboarding.cc
+++ b/selfdrive/ui/qt/offroad/onboarding.cc
@@ -29,14 +29,17 @@ void TrainingGuide::mouseReleaseEvent(QMouseEvent *e) {
   if (currentIndex >= (boundingRect.size() - 1)) {
     emit completedTraining();
   } else {
-    image.load(IMG_PATH + "step" + QString::number(currentIndex) + ".png");
+    image.load(img_path + "step" + QString::number(currentIndex) + ".png");
     update();
   }
 }
 
 void TrainingGuide::showEvent(QShowEvent *event) {
+  img_path = width() == WIDE_WIDTH ? "../assets/training_wide/" : "../assets/training/";
+  boundingRect = width() == WIDE_WIDTH ? boundingRectWide : boundingRectStandard;
+
   currentIndex = 0;
-  image.load(IMG_PATH + "step0.png");
+  image.load(img_path + "step0.png");
 }
 
 void TrainingGuide::paintEvent(QPaintEvent *event) {
@@ -151,8 +154,6 @@ void DeclinePage::showEvent(QShowEvent *event) {
 }
 
 void OnboardingWindow::updateActiveScreen() {
-  bool accepted_terms = params.get("HasAcceptedTerms") == current_terms_version;
-  bool training_done = params.get("CompletedTrainingVersion") == current_training_version;
   if (!accepted_terms) {
     setCurrentIndex(0);
   } else if (!training_done && !params.getBool("Passive")) {
@@ -163,13 +164,16 @@ void OnboardingWindow::updateActiveScreen() {
 }
 
 OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) {
-  current_terms_version = params.get("TermsVersion");
-  current_training_version = params.get("TrainingVersion");
+  std::string current_terms_version = params.get("TermsVersion");
+  std::string current_training_version = params.get("TrainingVersion");
+  accepted_terms = params.get("HasAcceptedTerms") == current_terms_version;
+  training_done = params.get("CompletedTrainingVersion") == current_training_version;
 
   TermsPage* terms = new TermsPage(this);
   addWidget(terms);
   connect(terms, &TermsPage::acceptedTerms, [=]() {
     Params().put("HasAcceptedTerms", current_terms_version);
+    accepted_terms = true;
     updateActiveScreen();
   });
   connect(terms, &TermsPage::declinedTerms, [=]() { setCurrentIndex(2); });
@@ -177,6 +181,7 @@ OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) {
   TrainingGuide* tr = new TrainingGuide(this);
   addWidget(tr);
   connect(tr, &TrainingGuide::completedTraining, [=]() {
+    training_done = true;
     Params().put("CompletedTrainingVersion", current_training_version);
     updateActiveScreen();
   });
@@ -198,8 +203,5 @@ OnboardingWindow::OnboardingWindow(QWidget *parent) : QStackedWidget(parent) {
       background-color: #4F4F4F;
     }
   )");
-}
-
-void OnboardingWindow::showEvent(QShowEvent *event) {
   updateActiveScreen();
 }
diff --git a/selfdrive/ui/qt/offroad/onboarding.h b/selfdrive/ui/qt/offroad/onboarding.h
index 980d450b7..7ae72649d 100644
--- a/selfdrive/ui/qt/offroad/onboarding.h
+++ b/selfdrive/ui/qt/offroad/onboarding.h
@@ -72,8 +72,8 @@ private:
     QRect(303, 755, 718, 189),
   };
 
-  const QString IMG_PATH = WIDE_UI ? "../assets/training_wide/" : "../assets/training/";
-  const QVector<QRect> boundingRect = WIDE_UI ? boundingRectWide : boundingRectStandard;
+  QString img_path;
+  QVector<QRect> boundingRect;
 
 signals:
   void completedTraining();
@@ -117,14 +117,14 @@ class OnboardingWindow : public QStackedWidget {
 
 public:
   explicit OnboardingWindow(QWidget *parent = 0);
+  inline void showTrainingGuide() { setCurrentIndex(1); }
+  inline bool completed() const { return accepted_terms && training_done; }
 
 private:
-  void showEvent(QShowEvent *event) override;
   void updateActiveScreen();
 
   Params params;
-  std::string current_terms_version;
-  std::string current_training_version;
+  bool accepted_terms = false, training_done = false;
 
 signals:
   void onboardingDone();
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index 1587cf8e1..35ae2fc91 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -25,40 +25,37 @@
 #include "selfdrive/ui/qt/util.h"
 #include "selfdrive/ui/qt/qt_window.h"
 
-TogglesPanel::TogglesPanel(QWidget *parent) : QWidget(parent) {
-  QVBoxLayout *main_layout = new QVBoxLayout(this);
-
-  QList<ParamControl*> toggles;
-
-  toggles.append(new ParamControl("OpenpilotEnabledToggle",
+TogglesPanel::TogglesPanel(QWidget *parent) : ListWidget(parent) {
+  auto params = Params();
+  addItem(new ParamControl("OpenpilotEnabledToggle",
                                   "Enable openpilot",
                                   "Use the openpilot system for adaptive cruise control and lane keep driver assistance. Your attention is required at all times to use this feature. Changing this setting takes effect when the car is powered off.",
                                   "../assets/offroad/icon_openpilot.png",
                                   this));
-  toggles.append(new ParamControl("IsLdwEnabled",
+  addItem(new ParamControl("IsLdwEnabled",
                                   "Enable Lane Departure Warnings",
                                   "Receive alerts to steer back into the lane when your vehicle drifts over a detected lane line without a turn signal activated while driving over 31mph (50kph).",
                                   "../assets/offroad/icon_warning.png",
                                   this));
-  toggles.append(new ParamControl("IsRHD",
+  addItem(new ParamControl("IsRHD",
                                   "Enable Right-Hand Drive",
                                   "Allow openpilot to obey left-hand traffic conventions and perform driver monitoring on right driver seat.",
                                   "../assets/offroad/icon_openpilot_mirrored.png",
                                   this));
-  toggles.append(new ParamControl("IsMetric",
+  addItem(new ParamControl("IsMetric",
                                   "Use Metric System",
-                                  "Display speed in km/h instead of mp/h.",
+                                  "Display speed in km/h instead of mph.",
                                   "../assets/offroad/icon_metric.png",
                                   this));
-  toggles.append(new ParamControl("CommunityFeaturesToggle",
+  addItem(new ParamControl("CommunityFeaturesToggle",
                                   "Enable Community Features",
-                                  "Use features from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. These features include community supported cars and community supported hardware. Be extra cautious when using these features",
+                                  "Use features, such as community supported hardware, from the open source community that are not maintained or supported by comma.ai and have not been confirmed to meet the standard safety model. Be extra cautious when using these features",
                                   "../assets/offroad/icon_shell.png",
                                   this));
 
-  toggles.append(new ParamControl("UploadRaw",
+  addItem(new ParamControl("UploadRaw",
                                   "Upload Raw Logs",
-                                  "Upload full logs and full resolution video by default while on WiFi. If not enabled, individual logs can be marked for upload at my.comma.ai/useradmin.",
+                                  "Upload full logs and full resolution video by default while on Wi-Fi. If not enabled, individual logs can be marked for upload at useradmin.comma.ai.",
                                   "../assets/offroad/icon_network.png",
                                   this));
 
@@ -67,40 +64,39 @@ TogglesPanel::TogglesPanel(QWidget *parent) : QWidget(parent) {
                                                  "Upload data from the driver facing camera and help improve the driver monitoring algorithm.",
                                                  "../assets/offroad/icon_monitoring.png",
                                                  this);
-  toggles.append(record_toggle);
-  toggles.append(new ParamControl("EndToEndToggle",
+  addItem(record_toggle);
+  addItem(new ParamControl("EndToEndToggle",
                                   "\U0001f96c Disable use of lanelines (Alpha) \U0001f96c",
                                   "In this mode openpilot will ignore lanelines and just drive how it thinks a human would.",
                                   "../assets/offroad/icon_road.png",
                                   this));
-
 #ifdef ENABLE_MAPS
-  toggles.append(new ParamControl("NavSettingTime24h",
+  addItem(new ParamControl("NavSettingTime24h",
                                   "Show ETA in 24h format",
                                   "Use 24h format instead of am/pm",
                                   "../assets/offroad/icon_metric.png",
                                   this));
 #endif
+  if (params.getBool("DisableRadar_Allow")) {
+    addItem(new ParamControl("DisableRadar",
+                             "openpilot Longitudinal Control",
+                             "openpilot will disable the car's radar and will take over control of gas and brakes. Warning: this disables AEB!",
+                             "../assets/offroad/icon_speed_limit.png",
+                             this));
 
-  bool record_lock = Params().getBool("RecordFrontLock");
-  record_toggle->setEnabled(!record_lock);
-
-  for(ParamControl *toggle : toggles) {
-    if(main_layout->count() != 0) {
-      main_layout->addWidget(horizontal_line());
-    }
-    main_layout->addWidget(toggle);
   }
+
+  bool record_lock = params.getBool("RecordFrontLock");
+  record_toggle->setEnabled(!record_lock);
 }
 
-DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) {
-  QVBoxLayout *main_layout = new QVBoxLayout(this);
+DevicePanel::DevicePanel(QWidget* parent) : ListWidget(parent) {
+  setSpacing(50);
   Params params = Params();
-  main_layout->addWidget(new LabelControl("Dongle ID", getDongleId().value_or("N/A")));
-  main_layout->addWidget(horizontal_line());
+  addItem(new LabelControl("Dongle ID", getDongleId().value_or("N/A")));
 
   QString serial = QString::fromStdString(params.get("HardwareSerial", false));
-  main_layout->addWidget(new LabelControl("Serial", serial));
+  addItem(new LabelControl("Serial", serial));
 
   // offroad-only buttons
 
@@ -142,7 +138,6 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) {
     retrainingBtn = new ButtonControl("Review Training Guide", "REVIEW", "Review the rules, features, and limitations of openpilot");
     connect(retrainingBtn, &ButtonControl::clicked, [=]() {
       if (ConfirmationDialog::confirm("Are you sure you want to review the training guide?", this)) {
-        Params().remove("CompletedTrainingVersion");
         emit reviewTrainingGuide();
       }
     });
@@ -152,16 +147,15 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) {
   if (Hardware::TICI()) {
     regulatoryBtn = new ButtonControl("Regulatory", "VIEW", "");
     connect(regulatoryBtn, &ButtonControl::clicked, [=]() {
-      const std::string txt = util::read_file(ASSET_PATH.toStdString() + "/offroad/fcc.html");
+      const std::string txt = util::read_file("../assets/offroad/fcc.html");
       RichTextDialog::alert(QString::fromStdString(txt), this);
     });
   }
 
   for (auto btn : {dcamBtn, resetCalibBtn, retrainingBtn, regulatoryBtn}) {
     if (btn) {
-      main_layout->addWidget(horizontal_line());
       connect(parent, SIGNAL(offroadTransition(bool)), btn, SLOT(setEnabled(bool)));
-      main_layout->addWidget(btn);
+      addItem(btn);
     }
   }
 
@@ -197,10 +191,10 @@ DevicePanel::DevicePanel(QWidget* parent) : QWidget(parent) {
     #poweroff_btn { background-color: #E22C2C; }
     #poweroff_btn:pressed { background-color: #FF2424; }
   )");
-  main_layout->addLayout(power_layout);
+  addItem(power_layout);
 }
 
-SoftwarePanel::SoftwarePanel(QWidget* parent) : QWidget(parent) {
+SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
   gitBranchLbl = new LabelControl("Git Branch");
   gitCommitLbl = new LabelControl("Git Commit");
   osVersionLbl = new LabelControl("OS Version");
@@ -217,12 +211,6 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : QWidget(parent) {
     std::system("pkill -1 -f selfdrive.updated");
   });
 
-  QVBoxLayout *main_layout = new QVBoxLayout(this);
-  QWidget *widgets[] = {versionLbl, lastUpdateLbl, updateBtn, gitBranchLbl, gitCommitLbl, osVersionLbl};
-  for (int i = 0; i < std::size(widgets); ++i) {
-    main_layout->addWidget(widgets[i]);
-    main_layout->addWidget(horizontal_line());
-  }
 
   auto uninstallBtn = new ButtonControl("Uninstall " + getBrand(), "UNINSTALL");
   connect(uninstallBtn, &ButtonControl::clicked, [=]() {
@@ -231,7 +219,11 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : QWidget(parent) {
     }
   });
   connect(parent, SIGNAL(offroadTransition(bool)), uninstallBtn, SLOT(setEnabled(bool)));
-  main_layout->addWidget(uninstallBtn);
+
+  QWidget *widgets[] = {versionLbl, lastUpdateLbl, updateBtn, gitBranchLbl, gitCommitLbl, osVersionLbl, uninstallBtn};
+  for (QWidget* w : widgets) {
+    addItem(w);
+  }
 
   fs_watch = new QFileSystemWatcher(this);
   QObject::connect(fs_watch, &QFileSystemWatcher::fileChanged, [=](const QString path) {
@@ -270,24 +262,24 @@ QWidget * network_panel(QWidget * parent) {
 #ifdef QCOM
   QWidget *w = new QWidget(parent);
   QVBoxLayout *layout = new QVBoxLayout(w);
-  layout->setSpacing(30);
+  layout->setContentsMargins(50, 0, 50, 0);
 
+  ListWidget *list = new ListWidget();
+  list->setSpacing(30);
   // wifi + tethering buttons
-  auto wifiBtn = new ButtonControl("WiFi Settings", "OPEN");
+  auto wifiBtn = new ButtonControl("Wi-Fi Settings", "OPEN");
   QObject::connect(wifiBtn, &ButtonControl::clicked, [=]() { HardwareEon::launch_wifi(); });
-  layout->addWidget(wifiBtn);
-  layout->addWidget(horizontal_line());
+  list->addItem(wifiBtn);
 
   auto tetheringBtn = new ButtonControl("Tethering Settings", "OPEN");
   QObject::connect(tetheringBtn, &ButtonControl::clicked, [=]() { HardwareEon::launch_tethering(); });
-  layout->addWidget(tetheringBtn);
-  layout->addWidget(horizontal_line());
+  list->addItem(tetheringBtn);
 
   // SSH key management
-  layout->addWidget(new SshToggle());
-  layout->addWidget(horizontal_line());
-  layout->addWidget(new SshControl());
+  list->addItem(new SshToggle());
+  list->addItem(new SshControl());
 
+  layout->addWidget(list);
   layout->addStretch(1);
 #else
   Networking *w = new Networking(parent);
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index 9670d8d90..f92246336 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -13,7 +13,7 @@
 
 // ********** settings window + top-level panels **********
 
-class DevicePanel : public QWidget {
+class DevicePanel : public ListWidget {
   Q_OBJECT
 public:
   explicit DevicePanel(QWidget* parent = nullptr);
@@ -22,13 +22,13 @@ signals:
   void showDriverView();
 };
 
-class TogglesPanel : public QWidget {
+class TogglesPanel : public ListWidget {
   Q_OBJECT
 public:
   explicit TogglesPanel(QWidget *parent = nullptr);
 };
 
-class SoftwarePanel : public QWidget {
+class SoftwarePanel : public ListWidget {
   Q_OBJECT
 public:
   explicit SoftwarePanel(QWidget* parent = nullptr);
diff --git a/selfdrive/ui/qt/offroad/wifiManager.cc b/selfdrive/ui/qt/offroad/wifiManager.cc
index c6eb27c6c..957d65e26 100644
--- a/selfdrive/ui/qt/offroad/wifiManager.cc
+++ b/selfdrive/ui/qt/offroad/wifiManager.cc
@@ -202,7 +202,7 @@ void WifiManager::connect(const QByteArray &ssid, const QString &username, const
   nm_settings.call("AddConnection", QVariant::fromValue(connection));
 }
 
-void WifiManager::deactivateConnection(const QString &ssid) {
+void WifiManager::deactivateConnectionBySsid(const QString &ssid) {
   for (QDBusObjectPath active_connection_raw : get_active_connections()) {
     QString active_connection = active_connection_raw.path();
     QDBusInterface nm(NM_DBUS_SERVICE, active_connection, NM_DBUS_INTERFACE_PROPERTIES, bus);
@@ -212,14 +212,18 @@ void WifiManager::deactivateConnection(const QString &ssid) {
     if (pth.path() != "" && pth.path() != "/") {
       QString Ssid = get_property(pth.path(), "Ssid");
       if (Ssid == ssid) {
-        QDBusInterface nm2(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus);
-        nm2.setTimeout(DBUS_TIMEOUT);
-        nm2.call("DeactivateConnection", QVariant::fromValue(active_connection_raw));
+        deactivateConnection(active_connection_raw);
       }
     }
   }
 }
 
+void WifiManager::deactivateConnection(const QDBusObjectPath &path) {
+  QDBusInterface nm2(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus);
+  nm2.setTimeout(DBUS_TIMEOUT);
+  nm2.call("DeactivateConnection", QVariant::fromValue(path));
+}
+
 QVector<QDBusObjectPath> WifiManager::get_active_connections() {
   QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, bus);
   nm.setTimeout(DBUS_TIMEOUT);
@@ -250,11 +254,14 @@ void WifiManager::forgetConnection(const QString &ssid) {
   }
 }
 
-bool WifiManager::isWirelessAdapter(const QDBusObjectPath &path) {
+uint WifiManager::getAdapterType(const QDBusObjectPath &path) {
   QDBusInterface device_props(NM_DBUS_SERVICE, path.path(), NM_DBUS_INTERFACE_PROPERTIES, bus);
   device_props.setTimeout(DBUS_TIMEOUT);
-  const uint deviceType = get_response<uint>(device_props.call("Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType"));
-  return deviceType == NM_DEVICE_TYPE_WIFI;
+  return get_response<uint>(device_props.call("Get", NM_DBUS_INTERFACE_DEVICE, "DeviceType"));
+}
+
+bool WifiManager::isWirelessAdapter(const QDBusObjectPath &path) {
+  return getAdapterType(path) == NM_DEVICE_TYPE_WIFI;
 }
 
 void WifiManager::requestScan() {
@@ -288,13 +295,13 @@ unsigned int WifiManager::get_ap_strength(const QString &network_path) {
   return get_response<unsigned int>(response);
 }
 
-QString WifiManager::getAdapter() {
+QString WifiManager::getAdapter(const uint adapter_type) {
   QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus);
   nm.setTimeout(DBUS_TIMEOUT);
 
   const QDBusReply<QList<QDBusObjectPath>> &response = nm.call("GetDevices");
   for (const QDBusObjectPath &path : response.value()) {
-    if (isWirelessAdapter(path)) {
+    if (getAdapterType(path) == adapter_type) {
       return path.path();
     }
   }
@@ -352,7 +359,7 @@ void WifiManager::newConnection(const QDBusObjectPath &path) {
 
 void WifiManager::disconnect() {
   if (activeAp != "" && activeAp != "/") {
-    deactivateConnection(get_property(activeAp, "Ssid"));
+    deactivateConnectionBySsid(get_property(activeAp, "Ssid"));
   }
 }
 
@@ -381,7 +388,7 @@ void WifiManager::initConnections() {
     if (settings.value("connection").value("type") == "802-11-wireless") {
       knownConnections[path] = settings.value("802-11-wireless").value("ssid").toString();
     } else if (path.path() != "/") {
-      lteConnectionPath = path.path();
+      lteConnectionPath = path;
     }
   }
 }
@@ -396,6 +403,15 @@ void WifiManager::activateWifiConnection(const QString &ssid) {
   }
 }
 
+void WifiManager::activateModemConnection(const QDBusObjectPath &path) {
+  QString modem = getAdapter(NM_DEVICE_TYPE_MODEM);
+  if (!path.path().isEmpty() && !modem.isEmpty()) {
+    QDBusInterface nm3(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE, bus);
+    nm3.setTimeout(DBUS_TIMEOUT);
+    nm3.call("ActivateConnection", QVariant::fromValue(path), QVariant::fromValue(QDBusObjectPath(modem)), QVariant::fromValue(QDBusObjectPath("/")));
+  }
+}
+
 // function matches tici/hardware.py
 NetworkType WifiManager::currentNetworkType() {
   QDBusInterface nm(NM_DBUS_SERVICE, NM_DBUS_PATH, NM_DBUS_INTERFACE_PROPERTIES, bus);
@@ -423,15 +439,37 @@ NetworkType WifiManager::currentNetworkType() {
   return NetworkType::NONE;
 }
 
-void WifiManager::setRoamingEnabled(bool roaming) {
-  if (!lteConnectionPath.isEmpty()) {
-    QDBusInterface nm(NM_DBUS_SERVICE, lteConnectionPath, NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus);
+void WifiManager::updateGsmSettings(bool roaming, QString apn) {
+  if (!lteConnectionPath.path().isEmpty()) {
+    QDBusInterface nm(NM_DBUS_SERVICE, lteConnectionPath.path(), NM_DBUS_INTERFACE_SETTINGS_CONNECTION, bus);
     nm.setTimeout(DBUS_TIMEOUT);
 
+    bool changes = false;
+    bool auto_config = apn.isEmpty();
     Connection settings = QDBusReply<Connection>(nm.call("GetSettings")).value();
+
+    if (settings.value("gsm").value("auto-config").toBool() != auto_config) {
+      qWarning() << "Changing gsm.auto-config to" << auto_config;
+      settings["gsm"]["auto-config"] = auto_config;
+      changes = true;
+    }
+
+    if (settings.value("gsm").value("apn").toString() != apn) {
+      qWarning() << "Changing gsm.apn to" << apn;
+      settings["gsm"]["apn"] = apn;
+      changes = true;
+    }
+
     if (settings.value("gsm").value("home-only").toBool() == roaming) {
+      qWarning() << "Changing gsm.home-only to" << !roaming;
       settings["gsm"]["home-only"] = !roaming;
+      changes = true;
+    }
+
+    if (changes) {
       nm.call("UpdateUnsaved", QVariant::fromValue(settings));  // update is temporary
+      deactivateConnection(lteConnectionPath);
+      activateModemConnection(lteConnectionPath);
     }
   }
 }
@@ -476,7 +514,7 @@ void WifiManager::setTetheringEnabled(bool enabled) {
     }
     activateWifiConnection(tethering_ssid);
   } else {
-    deactivateConnection(tethering_ssid);
+    deactivateConnectionBySsid(tethering_ssid);
   }
 }
 
diff --git a/selfdrive/ui/qt/offroad/wifiManager.h b/selfdrive/ui/qt/offroad/wifiManager.h
index 2fb539260..04defd32d 100644
--- a/selfdrive/ui/qt/offroad/wifiManager.h
+++ b/selfdrive/ui/qt/offroad/wifiManager.h
@@ -42,15 +42,16 @@ public:
   void requestScan();
   QMap<QString, Network> seenNetworks;
   QMap<QDBusObjectPath, QString> knownConnections;
-  QString lteConnectionPath;
+  QDBusObjectPath lteConnectionPath;
   QString ipv4_address;
 
   void refreshNetworks();
   void forgetConnection(const QString &ssid);
   bool isKnownConnection(const QString &ssid);
   void activateWifiConnection(const QString &ssid);
+  void activateModemConnection(const QDBusObjectPath &path);
   NetworkType currentNetworkType();
-  void setRoamingEnabled(bool roaming);
+  void updateGsmSettings(bool roaming, QString apn);
 
   void connect(const Network &ssid);
   void connect(const Network &ssid, const QString &password);
@@ -73,13 +74,15 @@ private:
   const QString defaultTetheringPassword = "swagswagcomma";
 
   bool firstScan = true;
-  QString getAdapter();
+  QString getAdapter(const uint = NM_DEVICE_TYPE_WIFI);
+  uint getAdapterType(const QDBusObjectPath &path);
   bool isWirelessAdapter(const QDBusObjectPath &path);
   QString get_ipv4_address();
   void connect(const QByteArray &ssid, const QString &username, const QString &password, SecurityType security_type);
   QString activeAp;
   void initActiveAp();
-  void deactivateConnection(const QString &ssid);
+  void deactivateConnectionBySsid(const QString &ssid);
+  void deactivateConnection(const QDBusObjectPath &path);
   QVector<QDBusObjectPath> get_active_connections();
   uint get_wifi_device_state();
   QByteArray get_property(const QString &network_path, const QString &property);
diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index 75593cda2..30bc7fe4f 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -1,9 +1,7 @@
 #include "selfdrive/ui/qt/onroad.h"
 
-#include <iostream>
 #include <QDebug>
 
-#include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/timing.h"
 #include "selfdrive/ui/paint.h"
 #include "selfdrive/ui/qt/util.h"
@@ -11,6 +9,7 @@
 #include "selfdrive/ui/qt/maps/map.h"
 #endif
 
+
 OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
   QVBoxLayout *main_layout  = new QVBoxLayout(this);
   main_layout->setMargin(bdr_s);
@@ -18,9 +17,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
   stacked_layout->setStackingMode(QStackedLayout::StackAll);
   main_layout->addLayout(stacked_layout);
 
-  // old UI on bottom
-  nvg = new NvgWindow(this);
-  QObject::connect(this, &OnroadWindow::updateStateSignal, nvg, &NvgWindow::updateState);
+  nvg = new NvgWindow(VISION_STREAM_RGB_BACK, this);
 
   QWidget * split_wrapper = new QWidget;
   split = new QHBoxLayout(split_wrapper);
@@ -100,6 +97,10 @@ void OnroadWindow::offroadTransition(bool offroad) {
 #endif
 
   alerts->updateAlert({}, bg);
+
+  // update stream type
+  bool wide_cam = Hardware::TICI() && Params().getBool("EnableWideCamera");
+  nvg->setStreamType(wide_cam ? VISION_STREAM_RGB_WIDE : VISION_STREAM_RGB_BACK);
 }
 
 void OnroadWindow::paintEvent(QPaintEvent *event) {
@@ -168,18 +169,8 @@ void OnroadAlerts::paintEvent(QPaintEvent *event) {
   }
 }
 
-
-NvgWindow::NvgWindow(QWidget *parent) : QOpenGLWidget(parent) {
-  setAttribute(Qt::WA_OpaquePaintEvent);
-}
-
-NvgWindow::~NvgWindow() {
-  makeCurrent();
-  doneCurrent();
-}
-
 void NvgWindow::initializeGL() {
-  initializeOpenGLFunctions();
+  CameraViewWidget::initializeGL();
   qInfo() << "OpenGL version:" << QString((const char*)glGetString(GL_VERSION));
   qInfo() << "OpenGL vendor:" << QString((const char*)glGetString(GL_VENDOR));
   qInfo() << "OpenGL renderer:" << QString((const char*)glGetString(GL_RENDERER));
@@ -187,24 +178,11 @@ void NvgWindow::initializeGL() {
 
   ui_nvg_init(&QUIState::ui_state);
   prev_draw_t = millis_since_boot();
-}
-
-void NvgWindow::updateState(const UIState &s) {
-  // Connecting to visionIPC requires opengl to be current
-  if (s.vipc_client->connected) {
-    makeCurrent();
-  }
-  if (isVisible() != s.vipc_client->connected) {
-    setVisible(s.vipc_client->connected);
-  }
-  repaint();
-}
-
-void NvgWindow::resizeGL(int w, int h) {
-  ui_resize(&QUIState::ui_state, w, h);
+  setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
 }
 
 void NvgWindow::paintGL() {
+  CameraViewWidget::paintGL();
   ui_draw(&QUIState::ui_state, width(), height());
 
   double cur_draw_t = millis_since_boot();
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index 7ef9dfcbe..9be04e04a 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -1,12 +1,9 @@
 #pragma once
 
-#include <QOpenGLFunctions>
-#include <QOpenGLWidget>
 #include <QStackedLayout>
 #include <QWidget>
 
-#include "cereal/gen/cpp/log.capnp.h"
-#include "selfdrive/ui/qt/qt_window.h"
+#include "selfdrive/ui/qt/widgets/cameraview.h"
 #include "selfdrive/ui/ui.h"
 
 
@@ -28,24 +25,16 @@ private:
 };
 
 // container window for the NVG UI
-class NvgWindow : public QOpenGLWidget, protected QOpenGLFunctions {
+class NvgWindow : public CameraViewWidget {
   Q_OBJECT
 
 public:
-  using QOpenGLWidget::QOpenGLWidget;
-  explicit NvgWindow(QWidget* parent = 0);
-  ~NvgWindow();
+  explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget(type, true, parent) {}
 
 protected:
   void paintGL() override;
   void initializeGL() override;
-  void resizeGL(int w, int h) override;
-
-private:
   double prev_draw_t = 0;
-
-public slots:
-  void updateState(const UIState &s);
 };
 
 // container for all onroad widgets
diff --git a/selfdrive/ui/qt/qt_window.h b/selfdrive/ui/qt/qt_window.h
index a2dcbb425..cd1ad928d 100644
--- a/selfdrive/ui/qt/qt_window.h
+++ b/selfdrive/ui/qt/qt_window.h
@@ -3,6 +3,7 @@
 #include <string>
 
 #include <QApplication>
+#include <QScreen>
 #include <QWidget>
 
 #ifdef QCOM2
@@ -15,13 +16,14 @@
 
 const QString ASSET_PATH = ":/";
 
-const bool WIDE_UI = Hardware::TICI() || getenv("WIDE_UI") != nullptr;
-const int vwp_w = WIDE_UI ? 2160 : 1920;
-const int vwp_h = 1080;
+const int WIDE_WIDTH = 2160;
 
 inline void setMainWindow(QWidget *w) {
+  const bool wide = (QGuiApplication::primaryScreen()->size().width() >= WIDE_WIDTH) ^
+                    (getenv("INVERT_WIDTH") != NULL);
   const float scale = util::getenv("SCALE", 1.0f);
-  w->setFixedSize(vwp_w*scale, vwp_h*scale);
+
+  w->setFixedSize(QSize(wide ? WIDE_WIDTH : 1920, 1080) * scale);
   w->show();
 
 #ifdef QCOM2
diff --git a/selfdrive/ui/qt/sidebar.cc b/selfdrive/ui/qt/sidebar.cc
index 9c158ac3e..cd53a5b72 100644
--- a/selfdrive/ui/qt/sidebar.cc
+++ b/selfdrive/ui/qt/sidebar.cc
@@ -59,12 +59,12 @@ void Sidebar::updateState(const UIState &s) {
   }
   setProperty("connectStatus", QVariant::fromValue(connectStatus));
 
-  ItemStatus tempStatus = {"HIGH\nTEMP", danger_color};
+  ItemStatus tempStatus = {"TEMP\nHIGH", danger_color};
   auto ts = deviceState.getThermalStatus();
   if (ts == cereal::DeviceState::ThermalStatus::GREEN) {
-    tempStatus = {"GOOD\nTEMP", good_color};
+    tempStatus = {"TEMP\nGOOD", good_color};
   } else if (ts == cereal::DeviceState::ThermalStatus::YELLOW) {
-    tempStatus = {"OK\nTEMP", warning_color};
+    tempStatus = {"TEMP\nOK", warning_color};
   }
   setProperty("tempStatus", QVariant::fromValue(tempStatus));
 
diff --git a/selfdrive/ui/qt/sidebar.h b/selfdrive/ui/qt/sidebar.h
index f22f1f732..6cea9bdb6 100644
--- a/selfdrive/ui/qt/sidebar.h
+++ b/selfdrive/ui/qt/sidebar.h
@@ -35,7 +35,7 @@ protected:
   QImage home_img, settings_img;
   const QMap<cereal::DeviceState::NetworkType, QString> network_type = {
     {cereal::DeviceState::NetworkType::NONE, "--"},
-    {cereal::DeviceState::NetworkType::WIFI, "WiFi"},
+    {cereal::DeviceState::NetworkType::WIFI, "Wi-Fi"},
     {cereal::DeviceState::NetworkType::ETHERNET, "ETH"},
     {cereal::DeviceState::NetworkType::CELL2_G, "2G"},
     {cereal::DeviceState::NetworkType::CELL3_G, "3G"},
diff --git a/selfdrive/ui/qt/spinner_aarch64 b/selfdrive/ui/qt/spinner_aarch64
index 77c13d81e..71a52bc9f 100755
Binary files a/selfdrive/ui/qt/spinner_aarch64 and b/selfdrive/ui/qt/spinner_aarch64 differ
diff --git a/selfdrive/ui/qt/spinner_larch64 b/selfdrive/ui/qt/spinner_larch64
index fa39f0ed5..645bc4430 100755
Binary files a/selfdrive/ui/qt/spinner_larch64 and b/selfdrive/ui/qt/spinner_larch64 differ
diff --git a/selfdrive/ui/qt/text_aarch64 b/selfdrive/ui/qt/text_aarch64
index f4f770afd..dbce56d59 100755
Binary files a/selfdrive/ui/qt/text_aarch64 and b/selfdrive/ui/qt/text_aarch64 differ
diff --git a/selfdrive/ui/qt/text_larch64 b/selfdrive/ui/qt/text_larch64
index bdfa10daf..eb0f535bf 100755
Binary files a/selfdrive/ui/qt/text_larch64 and b/selfdrive/ui/qt/text_larch64 differ
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index b9e39b8cf..f9fa56207 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -1,4 +1,3 @@
-#include "selfdrive/ui/qt/qt_window.h"
 #include "selfdrive/ui/qt/widgets/cameraview.h"
 
 namespace {
@@ -50,9 +49,9 @@ mat4 get_driver_view_transform() {
     // from dmonitoring.cc
     const int full_width_tici = 1928;
     const int full_height_tici = 1208;
-    const int adapt_width_tici = 668;
-    const int crop_x_offset = 32;
-    const int crop_y_offset = -196;
+    const int adapt_width_tici = 954;
+    const int crop_x_offset = -72;
+    const int crop_y_offset = -144;
     const float yscale = full_height_tici * driver_view_ratio / adapt_width_tici;
     const float xscale = yscale*(1080)/(2160)*full_width_tici/full_height_tici;
     transform = (mat4){{
@@ -96,8 +95,9 @@ CameraViewWidget::CameraViewWidget(VisionStreamType stream_type, bool zoom, QWid
                                    stream_type(stream_type), zoomed_view(zoom), QOpenGLWidget(parent) {
   setAttribute(Qt::WA_OpaquePaintEvent);
 
-  timer = new QTimer(this);
-  connect(timer, &QTimer::timeout, this, &CameraViewWidget::updateFrame);
+  QTimer *t = new QTimer(this);
+  connect(t, &QTimer::timeout, this, &CameraViewWidget::updateFrame);
+  t->start(10);
 }
 
 CameraViewWidget::~CameraViewWidget() {
@@ -113,9 +113,15 @@ CameraViewWidget::~CameraViewWidget() {
 void CameraViewWidget::initializeGL() {
   initializeOpenGLFunctions();
 
-  gl_shader = std::make_unique<GLShader>(frame_vertex_shader, frame_fragment_shader);
-  GLint frame_pos_loc = glGetAttribLocation(gl_shader->prog, "aPosition");
-  GLint frame_texcoord_loc = glGetAttribLocation(gl_shader->prog, "aTexCoord");
+  program = new QOpenGLShaderProgram(context());
+  bool ret = program->addShaderFromSourceCode(QOpenGLShader::Vertex, frame_vertex_shader);
+  assert(ret);
+  ret = program->addShaderFromSourceCode(QOpenGLShader::Fragment, frame_fragment_shader);
+  assert(ret);
+
+  program->link();
+  GLint frame_pos_loc = program->attributeLocation("aPosition");
+  GLint frame_texcoord_loc = program->attributeLocation("aTexCoord");
 
   auto [x1, x2, y1, y2] = stream_type == VISION_STREAM_RGB_FRONT ? std::tuple(0.f, 1.f, 1.f, 0.f) : std::tuple(1.f, 0.f, 1.f, 0.f);
   const uint8_t frame_indicies[] = {0, 1, 2, 0, 2, 3};
@@ -143,15 +149,11 @@ void CameraViewWidget::initializeGL() {
   glBindBuffer(GL_ARRAY_BUFFER, 0);
   glBindVertexArray(0);
 
-  vipc_client = std::make_unique<VisionIpcClient>("camerad", stream_type, true);
+  setStreamType(stream_type);
 }
 
-void CameraViewWidget::showEvent(QShowEvent *event) {
-  timer->start(0);
-}
 
 void CameraViewWidget::hideEvent(QHideEvent *event) {
-  timer->stop();
   vipc_client->connected = false;
   latest_frame = nullptr;
 }
@@ -161,10 +163,22 @@ void CameraViewWidget::mouseReleaseEvent(QMouseEvent *event) {
 }
 
 void CameraViewWidget::resizeGL(int w, int h) {
-  if (!vipc_client->connected) {
-    return;
-  }
+  updateFrameMat(w, h);
+}
 
+void CameraViewWidget::setStreamType(VisionStreamType type) {
+  if (!vipc_client || type != stream_type) {
+    stream_type = type;
+    vipc_client.reset(new VisionIpcClient("camerad", stream_type, true));
+    updateFrameMat(width(), height());
+  }
+}
+
+void CameraViewWidget::setBackgroundColor(QColor color) {
+  bg = color;
+}
+
+void CameraViewWidget::updateFrameMat(int w, int h) {
   if (zoomed_view) {
     if (stream_type == VISION_STREAM_RGB_FRONT) {
       frame_mat = matmul(device_transform, get_driver_view_transform());
@@ -185,7 +199,7 @@ void CameraViewWidget::resizeGL(int w, int h) {
       }};
       frame_mat = matmul(device_transform, frame_transform);
     }
-  } else {
+  } else if (vipc_client->connected) {
     // fit frame to widget size
     float w  = (float)width() / height();
     float f = (float)vipc_client->buffers[0].width  / vipc_client->buffers[0].height;
@@ -195,7 +209,7 @@ void CameraViewWidget::resizeGL(int w, int h) {
 
 void CameraViewWidget::paintGL() {
   if (!latest_frame) {
-    glClearColor(0, 0, 0, 1.0);
+    glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
     glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
     return;
   }
@@ -212,9 +226,9 @@ void CameraViewWidget::paintGL() {
                   0, GL_RGB, GL_UNSIGNED_BYTE, latest_frame->addr);
   }
 
-  glUseProgram(gl_shader->prog);
-  glUniform1i(gl_shader->getUniformLocation("uTexture"), 0);
-  glUniformMatrix4fv(gl_shader->getUniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
+  glUseProgram(program->programId());
+  glUniform1i(program->uniformLocation("uTexture"), 0);
+  glUniformMatrix4fv(program->uniformLocation("uTransform"), 1, GL_TRUE, frame_mat.v);
 
   assert(glGetError() == GL_NO_ERROR);
   glEnableVertexAttribArray(0);
@@ -224,33 +238,39 @@ void CameraViewWidget::paintGL() {
 }
 
 void CameraViewWidget::updateFrame() {
-  if (!vipc_client->connected && vipc_client->connect(false)) {
-    // init vision
-    for (int i = 0; i < vipc_client->num_buffers; i++) {
-      texture[i].reset(new EGLImageTexture(&vipc_client->buffers[i]));
-
-      glBindTexture(GL_TEXTURE_2D, texture[i]->frame_tex);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-
-      // BGR
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
-      glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
-      assert(glGetError() == GL_NO_ERROR);
-    }
-    latest_frame = nullptr;
-    resizeGL(width(), height());
+  if (!isVisible()) {
+    return;
   }
 
+  if (!vipc_client->connected) {
+    makeCurrent();
+    if (vipc_client->connect(false)) {
+      // init vision
+      for (int i = 0; i < vipc_client->num_buffers; i++) {
+        texture[i].reset(new EGLImageTexture(&vipc_client->buffers[i]));
+
+        glBindTexture(GL_TEXTURE_2D, texture[i]->frame_tex);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+
+        // BGR
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
+        assert(glGetError() == GL_NO_ERROR);
+      }
+      latest_frame = nullptr;
+      resizeGL(width(), height());
+    }
+  }
+
+  VisionBuf *buf = nullptr;
   if (vipc_client->connected) {
-    VisionBuf *buf = vipc_client->recv();
+    buf = vipc_client->recv(nullptr, 0);
     if (buf != nullptr) {
       latest_frame = buf;
       update();
       emit frameUpdated();
-    } else {
-      LOGE("visionIPC receive timeout");
     }
   }
 }
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 03faf2f3f..3bf34ac58 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -3,10 +3,10 @@
 #include <memory>
 
 #include <QOpenGLFunctions>
+#include <QOpenGLShaderProgram>
 #include <QOpenGLWidget>
 
 #include "cereal/visionipc/visionipc_client.h"
-#include "selfdrive/common/glutil.h"
 #include "selfdrive/common/mat.h"
 #include "selfdrive/common/visionimg.h"
 #include "selfdrive/ui/ui.h"
@@ -18,6 +18,8 @@ public:
   using QOpenGLWidget::QOpenGLWidget;
   explicit CameraViewWidget(VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr);
   ~CameraViewWidget();
+  void setStreamType(VisionStreamType type);
+  void setBackgroundColor(QColor color);
 
 signals:
   void clicked();
@@ -27,9 +29,10 @@ protected:
   void paintGL() override;
   void resizeGL(int w, int h) override;
   void initializeGL() override;
-  void showEvent(QShowEvent *event) override;
   void hideEvent(QHideEvent *event) override;
   void mouseReleaseEvent(QMouseEvent *event) override;
+  void updateFrameMat(int w, int h);
+  std::unique_ptr<VisionIpcClient> vipc_client;
 
 protected slots:
   void updateFrame();
@@ -39,10 +42,9 @@ private:
   VisionBuf *latest_frame = nullptr;
   GLuint frame_vao, frame_vbo, frame_ibo;
   mat4 frame_mat;
-  std::unique_ptr<VisionIpcClient> vipc_client;
   std::unique_ptr<EGLImageTexture> texture[UI_BUF_COUNT];
-  std::unique_ptr<GLShader> gl_shader;
+  QOpenGLShaderProgram *program;
 
-  QTimer* timer;
   VisionStreamType stream_type;
+  QColor bg = QColor("#000000");
 };
diff --git a/selfdrive/ui/qt/widgets/controls.cc b/selfdrive/ui/qt/widgets/controls.cc
index 2cc68be8b..d103db40d 100644
--- a/selfdrive/ui/qt/widgets/controls.cc
+++ b/selfdrive/ui/qt/widgets/controls.cc
@@ -18,6 +18,8 @@ QFrame *horizontal_line(QWidget *parent) {
 }
 
 AbstractControl::AbstractControl(const QString &title, const QString &desc, const QString &icon, QWidget *parent) : QFrame(parent) {
+  setSizePolicy(QSizePolicy::Preferred, QSizePolicy::Minimum);
+  
   QVBoxLayout *main_layout = new QVBoxLayout(this);
   main_layout->setMargin(0);
 
@@ -36,7 +38,8 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons
 
   // title
   title_label = new QPushButton(title);
-  title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left;");
+  title_label->setFixedHeight(120);
+  title_label->setStyleSheet("font-size: 50px; font-weight: 400; text-align: left");
   hlayout->addWidget(title_label);
 
   main_layout->addLayout(hlayout);
@@ -57,6 +60,7 @@ AbstractControl::AbstractControl(const QString &title, const QString &desc, cons
       description->setVisible(!description->isVisible());
     });
   }
+  main_layout->addStretch();
 }
 
 void AbstractControl::hideEvent(QHideEvent *e) {
diff --git a/selfdrive/ui/qt/widgets/controls.h b/selfdrive/ui/qt/widgets/controls.h
index bfa25ce83..b79332605 100644
--- a/selfdrive/ui/qt/widgets/controls.h
+++ b/selfdrive/ui/qt/widgets/controls.h
@@ -3,6 +3,7 @@
 #include <QFrame>
 #include <QHBoxLayout>
 #include <QLabel>
+#include <QPainter>
 #include <QPushButton>
 
 #include "selfdrive/common/params.h"
@@ -47,12 +48,6 @@ protected:
   AbstractControl(const QString &title, const QString &desc = "", const QString &icon = "", QWidget *parent = nullptr);
   void hideEvent(QHideEvent *e) override;
 
-  QSize minimumSizeHint() const override {
-    QSize size = QFrame::minimumSizeHint();
-    size.setHeight(120);
-    return size;
-  };
-
   QHBoxLayout *hlayout;
   QPushButton *title_label;
   QLabel *description = nullptr;
@@ -137,3 +132,32 @@ private:
   std::string key;
   Params params;
 };
+
+class ListWidget : public QWidget {
+  Q_OBJECT
+ public:
+  explicit ListWidget(QWidget *parent = 0) : QWidget(parent), outer_layout(this) {
+    outer_layout.setMargin(0);
+    outer_layout.setSpacing(0);
+    outer_layout.addLayout(&inner_layout);
+    inner_layout.setMargin(0);
+    inner_layout.setSpacing(25); // default spacing is 25
+    outer_layout.addStretch();
+  }
+  inline void addItem(QWidget *w) { inner_layout.addWidget(w); }
+  inline void addItem(QLayout *layout) { inner_layout.addLayout(layout); }
+  inline void setSpacing(int spacing) { inner_layout.setSpacing(spacing); }
+
+private:
+  void paintEvent(QPaintEvent *) override {
+    QPainter p(this);
+    p.setPen(Qt::gray);
+    for (int i = 0; i < inner_layout.count() - 1; ++i) {
+      QRect r = inner_layout.itemAt(i)->geometry();
+      int bottom = r.bottom() + inner_layout.spacing() / 2;
+      p.drawLine(r.left() + 40, bottom, r.right() - 40, bottom);
+    }
+  }
+  QVBoxLayout outer_layout;
+  QVBoxLayout inner_layout;
+};
diff --git a/selfdrive/ui/qt/widgets/input.cc b/selfdrive/ui/qt/widgets/input.cc
index f23d9b8b8..1122faf4c 100644
--- a/selfdrive/ui/qt/widgets/input.cc
+++ b/selfdrive/ui/qt/widgets/input.cc
@@ -32,7 +32,6 @@ QDialogBase::QDialogBase(QWidget *parent) : QDialog(parent) {
     QPushButton:pressed {
       background-color: #444444;
     }
-
   )");
 }
 
diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc
index e3828f3d8..2be91b5d1 100644
--- a/selfdrive/ui/qt/widgets/prime.cc
+++ b/selfdrive/ui/qt/widgets/prime.cc
@@ -8,21 +8,18 @@
 #include <QStackedWidget>
 #include <QTimer>
 #include <QVBoxLayout>
+
 #include <QrCode.hpp>
 
 #include "selfdrive/ui/qt/request_repeater.h"
 #include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/qt_window.h"
 
 using qrcodegen::QrCode;
 
 PairingQRWidget::PairingQRWidget(QWidget* parent) : QWidget(parent) {
-  qrCode = new QLabel;
-  qrCode->setScaledContents(true);
-  QVBoxLayout* main_layout = new QVBoxLayout(this);
-  main_layout->addWidget(qrCode, 0, Qt::AlignCenter);
-
   QTimer* timer = new QTimer(this);
-  timer->start(30 * 1000);
+  timer->start(5 * 60 * 1000);
   connect(timer, &QTimer::timeout, this, &PairingQRWidget::refresh);
 }
 
@@ -31,35 +28,86 @@ void PairingQRWidget::showEvent(QShowEvent *event) {
 }
 
 void PairingQRWidget::refresh() {
-  QString pairToken = CommaApi::create_jwt({{"pair", true}});
-  QString qrString = "https://connect.comma.ai/?pair=" + pairToken;
-  this->updateQrCode(qrString);
+  if (isVisible()) {
+    QString pairToken = CommaApi::create_jwt({{"pair", true}});
+    QString qrString = "https://connect.comma.ai/?pair=" + pairToken;
+    this->updateQrCode(qrString);
+  }
 }
 
 void PairingQRWidget::updateQrCode(const QString &text) {
   QrCode qr = QrCode::encodeText(text.toUtf8().data(), QrCode::Ecc::LOW);
   qint32 sz = qr.getSize();
-  // make the image larger so we can have a white border
-  QImage im(sz + 2, sz + 2, QImage::Format_RGB32);
+  QImage im(sz, sz, QImage::Format_RGB32);
+
   QRgb black = qRgb(0, 0, 0);
   QRgb white = qRgb(255, 255, 255);
-
-  for (int y = 0; y < sz + 2; y++) {
-    for (int x = 0; x < sz + 2; x++) {
-      im.setPixel(x, y, white);
-    }
-  }
   for (int y = 0; y < sz; y++) {
     for (int x = 0; x < sz; x++) {
-      im.setPixel(x + 1, y + 1, qr.getModule(x, y) ? black : white);
+      im.setPixel(x, y, qr.getModule(x, y) ? black : white);
     }
   }
+
   // Integer division to prevent anti-aliasing
-  int approx500 = (500 / (sz + 2)) * (sz + 2);
-  qrCode->setPixmap(QPixmap::fromImage(im.scaled(approx500, approx500, Qt::KeepAspectRatio, Qt::FastTransformation), Qt::MonoOnly));
-  qrCode->setFixedSize(approx500, approx500);
+  int final_sz = ((width() / sz) - 1) * sz;
+  img = QPixmap::fromImage(im.scaled(final_sz, final_sz, Qt::KeepAspectRatio), Qt::MonoOnly);
 }
 
+void PairingQRWidget::paintEvent(QPaintEvent *e) {
+  QPainter p(this);
+  p.fillRect(rect(), Qt::white);
+
+  QSize s = (size() - img.size()) / 2;
+  p.drawPixmap(s.width(), s.height(), img);
+}
+
+
+PairingPopup::PairingPopup(QWidget *parent) : QDialogBase(parent) {
+  QHBoxLayout *hlayout = new QHBoxLayout(this);
+  hlayout->setContentsMargins(0, 0, 0, 0);
+  hlayout->setSpacing(0);
+
+  setStyleSheet("PairingPopup { background-color: #E0E0E0; }");
+
+  // text
+  QVBoxLayout *vlayout = new QVBoxLayout();
+  vlayout->setContentsMargins(85, 70, 50, 70);
+  vlayout->setSpacing(50);
+  hlayout->addLayout(vlayout, 1);
+  {
+    QPushButton *close = new QPushButton(QIcon(":/icons/close.svg"), "", this);
+    close->setIconSize(QSize(80, 80));
+    close->setStyleSheet("border: none;");
+    vlayout->addWidget(close, 0, Qt::AlignLeft);
+    QObject::connect(close, &QPushButton::clicked, this, &QDialog::reject);
+
+    vlayout->addSpacing(30);
+
+    QLabel *title = new QLabel("Pair your device to your comma account", this);
+    title->setStyleSheet("font-size: 75px; color: black;");
+    title->setWordWrap(true);
+    vlayout->addWidget(title);
+
+    QLabel *instructions = new QLabel(R"(
+      <ol type='1' style='margin-left: 15px;'>
+        <li style='margin-bottom: 50px;'>Go to https://connect.comma.ai on your phone</li>
+        <li style='margin-bottom: 50px;'>Click "add new device" and scan the QR code on the right</li>
+        <li style='margin-bottom: 50px;'>Bookmark connect.comma.ai to your home screen to use it like an app</li>
+      </ol>
+    )", this);
+    instructions->setStyleSheet("font-size: 47px; font-weight: bold; color: black;");
+    instructions->setWordWrap(true);
+    vlayout->addWidget(instructions);
+
+    vlayout->addStretch();
+  }
+
+  // QR code
+  PairingQRWidget *qr = new PairingQRWidget(this);
+  hlayout->addWidget(qr, 1);
+}
+
+
 PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) {
   mainLayout = new QVBoxLayout(this);
   mainLayout->setMargin(0);
@@ -101,7 +149,7 @@ PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) {
   commaPoints->setStyleSheet("font-size: 41px; font-family: Inter SemiBold;");
   pointsLayout->addWidget(commaPoints, 0, Qt::AlignTop);
 
-  points = new QLabel("210");
+  points = new QLabel("0");
   points->setStyleSheet("font-size: 91px; font-weight: bold;");
   pointsLayout->addWidget(points, 0, Qt::AlignTop);
 
@@ -192,9 +240,9 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
 
   finishRegistationLayout->addStretch();
 
-  QPushButton* finishButton = new QPushButton("Pair device");
-  finishButton->setFixedHeight(220);
-  finishButton->setStyleSheet(R"(
+  QPushButton* pair = new QPushButton("Pair device");
+  pair->setFixedHeight(220);
+  pair->setStyleSheet(R"(
     QPushButton {
       font-size: 55px;
       font-weight: 400;
@@ -205,34 +253,18 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
       background-color: #3049F4;
     }
   )");
-  finishRegistationLayout->addWidget(finishButton);
-  QObject::connect(finishButton, &QPushButton::clicked, this, &SetupWidget::showQrCode);
+  finishRegistationLayout->addWidget(pair);
+
+  popup = new PairingPopup(this);
+  QObject::connect(pair, &QPushButton::clicked, popup, &PairingPopup::exec);
 
   mainLayout->addWidget(finishRegistration);
 
-  // Pairing QR code layout
-
-  QWidget* q = new QWidget;
-  q->setObjectName("primeWidget");
-  QVBoxLayout* qrLayout = new QVBoxLayout(q);
-  qrLayout->setContentsMargins(90, 90, 90, 90);
-
-  QLabel* qrLabel = new QLabel("Scan the QR code to pair.");
-  qrLabel->setAlignment(Qt::AlignHCenter);
-  qrLabel->setStyleSheet("font-size: 47px; font-weight: light;");
-  qrLayout->addWidget(qrLabel);
-  qrLayout->addSpacing(50);
-
-  qrLayout->addWidget(new PairingQRWidget);
-  qrLayout->addStretch();
-
-  // setup widget
+  // build stacked layout
   QVBoxLayout *outer_layout = new QVBoxLayout(this);
   outer_layout->setContentsMargins(0, 0, 0, 0);
   outer_layout->addWidget(mainLayout);
 
-  mainLayout->addWidget(q);
-
   primeAd = new PrimeAdWidget;
   mainLayout->addWidget(primeAd);
 
@@ -259,27 +291,15 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
     QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/";
     RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_Device", 5);
 
+    QObject::connect(repeater, &RequestRepeater::failedResponse, this, &SetupWidget::show);
     QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &SetupWidget::replyFinished);
-    QObject::connect(repeater, &RequestRepeater::failedResponse, this, &SetupWidget::parseError);
   }
   hide(); // Only show when first request comes back
 }
 
-void SetupWidget::parseError(const QString &response) {
-  show();
-  if (mainLayout->currentIndex() == 1) {
-    showQr = false;
-    mainLayout->setCurrentIndex(0);
-  }
-}
-
-void SetupWidget::showQrCode() {
-  showQr = true;
-  mainLayout->setCurrentIndex(1);
-}
-
 void SetupWidget::replyFinished(const QString &response) {
   show();
+
   QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8());
   if (doc.isNull()) {
     qDebug() << "JSON Parse failed on getting pairing and prime status";
@@ -288,12 +308,13 @@ void SetupWidget::replyFinished(const QString &response) {
 
   QJsonObject json = doc.object();
   if (!json["is_paired"].toBool()) {
-    mainLayout->setCurrentIndex(showQr);
-  } else if (!json["prime"].toBool()) {
-    showQr = false;
-    mainLayout->setCurrentWidget(primeAd);
+    mainLayout->setCurrentIndex(0);
   } else {
-    showQr = false;
-    mainLayout->setCurrentWidget(primeUser);
+    popup->reject();
+    if (!json["prime"].toBool()) {
+      mainLayout->setCurrentWidget(primeAd);
+    } else {
+      mainLayout->setCurrentWidget(primeUser);
+    }
   }
 }
diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h
index 27b2b6e54..6044f0610 100644
--- a/selfdrive/ui/qt/widgets/prime.h
+++ b/selfdrive/ui/qt/widgets/prime.h
@@ -5,14 +5,18 @@
 #include <QVBoxLayout>
 #include <QWidget>
 
+#include "selfdrive/ui/qt/widgets/input.h"
+
+// pairing QR code
 class PairingQRWidget : public QWidget {
   Q_OBJECT
 
 public:
   explicit PairingQRWidget(QWidget* parent = 0);
+  void paintEvent(QPaintEvent*) override;
 
 private:
-  QLabel* qrCode;
+  QPixmap img;
   void updateQrCode(const QString &text);
   void showEvent(QShowEvent *event) override;
 
@@ -20,6 +24,15 @@ private slots:
   void refresh();
 };
 
+// pairing popup widget
+class PairingPopup : public QDialogBase {
+  Q_OBJECT
+
+public:
+  explicit PairingPopup(QWidget* parent);
+};
+
+// widget for paired users with prime
 class PrimeUserWidget : public QWidget {
   Q_OBJECT
 public:
@@ -33,12 +46,15 @@ private slots:
   void replyFinished(const QString &response);
 };
 
+
+// widget for paired users without prime
 class PrimeAdWidget : public QFrame {
   Q_OBJECT
 public:
   explicit PrimeAdWidget(QWidget* parent = 0);
 };
 
+// container widget
 class SetupWidget : public QFrame {
   Q_OBJECT
 
@@ -46,13 +62,11 @@ public:
   explicit SetupWidget(QWidget* parent = 0);
 
 private:
-  QStackedWidget* mainLayout;
+  PairingPopup *popup;
+  QStackedWidget *mainLayout;
   PrimeAdWidget *primeAd;
   PrimeUserWidget *primeUser;
-  bool showQr = false;
 
 private slots:
-  void parseError(const QString &response);
   void replyFinished(const QString &response);
-  void showQrCode();
 };
diff --git a/selfdrive/ui/qt/window.cc b/selfdrive/ui/qt/window.cc
index 787fe4eba..ff8e9acc7 100644
--- a/selfdrive/ui/qt/window.cc
+++ b/selfdrive/ui/qt/window.cc
@@ -8,12 +8,6 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) {
   main_layout = new QStackedLayout(this);
   main_layout->setMargin(0);
 
-  onboardingWindow = new OnboardingWindow(this);
-  main_layout->addWidget(onboardingWindow);
-  QObject::connect(onboardingWindow, &OnboardingWindow::onboardingDone, [=]() {
-    main_layout->setCurrentWidget(homeWindow);
-  });
-
   homeWindow = new HomeWindow(this);
   main_layout->addWidget(homeWindow);
   QObject::connect(homeWindow, &HomeWindow::openSettings, this, &MainWindow::openSettings);
@@ -28,12 +22,22 @@ MainWindow::MainWindow(QWidget *parent) : QWidget(parent) {
   QObject::connect(settingsWindow, &SettingsWindow::closeSettings, this, &MainWindow::closeSettings);
   QObject::connect(&qs, &QUIState::offroadTransition, settingsWindow, &SettingsWindow::offroadTransition);
   QObject::connect(settingsWindow, &SettingsWindow::reviewTrainingGuide, [=]() {
+    onboardingWindow->showTrainingGuide();
     main_layout->setCurrentWidget(onboardingWindow);
   });
   QObject::connect(settingsWindow, &SettingsWindow::showDriverView, [=] {
     homeWindow->showDriverView(true);
   });
 
+  onboardingWindow = new OnboardingWindow(this);
+  main_layout->addWidget(onboardingWindow);
+  QObject::connect(onboardingWindow, &OnboardingWindow::onboardingDone, [=]() {
+    main_layout->setCurrentWidget(homeWindow);
+  });
+  if (!onboardingWindow->completed()) {
+    main_layout->setCurrentWidget(onboardingWindow);
+  }
+
   device.setAwake(true, true);
   QObject::connect(&qs, &QUIState::uiUpdate, &device, &Device::update);
   QObject::connect(&qs, &QUIState::offroadTransition, [=](bool offroad) {
diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc
new file mode 100644
index 000000000..aeef1a8e6
--- /dev/null
+++ b/selfdrive/ui/replay/framereader.cc
@@ -0,0 +1,166 @@
+#include "selfdrive/ui/replay/framereader.h"
+
+#include <unistd.h>
+#include <cassert>
+#include <mutex>
+#include "libyuv.h"
+
+static int ffmpeg_lockmgr_cb(void **arg, enum AVLockOp op) {
+  std::mutex *mutex = (std::mutex *)*arg;
+  switch (op) {
+  case AV_LOCK_CREATE:
+    mutex = new std::mutex();
+    break;
+  case AV_LOCK_OBTAIN:
+    mutex->lock();
+    break;
+  case AV_LOCK_RELEASE:
+    mutex->unlock();
+  case AV_LOCK_DESTROY:
+    delete mutex;
+    break;
+  }
+  return 0;
+}
+
+struct AVInitializer {
+  AVInitializer() {
+    int ret = av_lockmgr_register(ffmpeg_lockmgr_cb);
+    assert(ret >= 0);
+    av_register_all();
+    avformat_network_init();
+  }
+  ~AVInitializer() { avformat_network_deinit(); }
+};
+
+FrameReader::FrameReader() {
+  static AVInitializer av_initializer;
+}
+
+FrameReader::~FrameReader() {
+  for (auto &f : frames_) {
+    av_free_packet(&f.pkt);
+  }
+  if (pCodecCtx_) {
+    avcodec_close(pCodecCtx_);
+    avcodec_free_context(&pCodecCtx_);
+  }
+  if (pFormatCtx_) {
+    avformat_close_input(&pFormatCtx_);
+  }
+  if (av_frame_) {
+    av_frame_free(&av_frame_);
+  }
+}
+
+bool FrameReader::load(const std::string &url) {
+  pFormatCtx_ = avformat_alloc_context();
+  pFormatCtx_->probesize = 10 * 1024 * 1024;  // 10MB
+  if (avformat_open_input(&pFormatCtx_, url.c_str(), NULL, NULL) != 0) {
+    printf("error loading %s\n", url.c_str());
+    return false;
+  }
+  avformat_find_stream_info(pFormatCtx_, NULL);
+  // av_dump_format(pFormatCtx_, 0, url.c_str(), 0);
+
+  auto pCodecCtxOrig = pFormatCtx_->streams[0]->codec;
+  auto pCodec = avcodec_find_decoder(pCodecCtxOrig->codec_id);
+  if (!pCodec) return false;
+
+  pCodecCtx_ = avcodec_alloc_context3(pCodec);
+  int ret = avcodec_copy_context(pCodecCtx_, pCodecCtxOrig);
+  if (ret != 0) return false;
+
+  pCodecCtx_->thread_count = 0;
+  pCodecCtx_->thread_type = FF_THREAD_FRAME;
+  ret = avcodec_open2(pCodecCtx_, pCodec, NULL);
+  if (ret < 0) return false;
+
+  av_frame_ = av_frame_alloc();
+
+  width = pCodecCtxOrig->width;
+  height = pCodecCtxOrig->height;
+
+  frames_.reserve(60 * 20);  // 20fps, one minute
+  while (true) {
+    Frame &frame = frames_.emplace_back();
+    int err = av_read_frame(pFormatCtx_, &frame.pkt);
+    if (err < 0) {
+      frames_.pop_back();
+      valid_ = (err == AVERROR_EOF);
+      break;
+    }
+    // some stream seems to contian no keyframes
+    key_frames_count_ += frame.pkt.flags & AV_PKT_FLAG_KEY;
+  }
+  return valid_;
+}
+
+bool FrameReader::get(int idx, uint8_t *rgb, uint8_t *yuv) {
+  assert(rgb != nullptr);
+  if (!valid_ || idx < 0 || idx >= frames_.size()) {
+    return false;
+  }
+  return decode(idx, rgb, yuv);
+}
+
+bool FrameReader::decode(int idx, uint8_t *rgb, uint8_t *yuv) {
+  auto get_keyframe = [=](int idx) {
+    for (int i = idx; i >= 0 && key_frames_count_ > 1; --i) {
+      if (frames_[i].pkt.flags & AV_PKT_FLAG_KEY) return i;
+    }
+    return idx;
+  };
+
+  int from_idx = idx;
+  if (idx > 0 && !frames_[idx].decoded && !frames_[idx - 1].decoded) {
+    // find the previous keyframe
+    from_idx = get_keyframe(idx);
+  }
+
+  for (int i = from_idx; i <= idx; ++i) {
+    Frame &frame = frames_[i];
+    if ((!frame.decoded || i == idx) && !frame.failed) {
+      while (true) {
+        int ret = avcodec_decode_video2(pCodecCtx_, av_frame_, &frame.decoded, &(frame.pkt));
+        if (ret > 0 && !frame.decoded) {
+          // decode thread is still receiving the initial packets
+          usleep(0);
+        } else {
+          break;
+        }
+      }
+      frame.failed = !frame.decoded;
+      if (frame.decoded && i == idx) {
+        return decodeFrame(av_frame_, rgb, yuv);
+      }
+    }
+  }
+  return false;
+}
+
+bool FrameReader::decodeFrame(AVFrame *f, uint8_t *rgb, uint8_t *yuv) {
+  uint8_t *y = yuv;
+  if (!yuv) {
+    if (yuv_buf_.empty()) {
+      yuv_buf_.resize(getYUVSize());
+    }
+    y = yuv_buf_.data();
+  }
+
+  int i, j, k;
+  for (i = 0; i < f->height; i++) {
+    memcpy(y + f->width * i, f->data[0] + f->linesize[0] * i, f->width);
+  }
+  for (j = 0; j < f->height / 2; j++) {
+    memcpy(y + f->width * i + f->width / 2 * j, f->data[1] + f->linesize[1] * j, f->width / 2);
+  }
+  for (k = 0; k < f->height / 2; k++) {
+    memcpy(y + f->width * i + f->width / 2 * j + f->width / 2 * k, f->data[2] + f->linesize[2] * k, f->width / 2);
+  }
+
+  uint8_t *u = y + f->width * f->height;
+  uint8_t *v = u + (f->width / 2) * (f->height / 2);
+  libyuv::I420ToRGB24(y, f->width, u, f->width / 2, v, f->width / 2, rgb, f->width * 3, f->width, f->height);
+  return true;
+}
diff --git a/selfdrive/ui/replay/framereader.h b/selfdrive/ui/replay/framereader.h
new file mode 100644
index 000000000..81459618a
--- /dev/null
+++ b/selfdrive/ui/replay/framereader.h
@@ -0,0 +1,40 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+}
+
+class FrameReader {
+public:
+  FrameReader();
+  ~FrameReader();
+  bool load(const std::string &url);
+  bool get(int idx, uint8_t *rgb, uint8_t *yuv);
+  int getRGBSize() const { return width * height * 3; }
+  int getYUVSize() const { return width * height * 3 / 2; }
+  size_t getFrameCount() const { return frames_.size(); }
+  bool valid() const { return valid_; }
+
+  int width = 0, height = 0;
+
+private:
+  bool decode(int idx, uint8_t *rgb, uint8_t *yuv);
+  bool decodeFrame(AVFrame *f, uint8_t *rgb, uint8_t *yuv);
+
+  struct Frame {
+    AVPacket pkt = {};
+    int decoded = false;
+    bool failed = false;
+  };
+  std::vector<Frame> frames_;
+  AVFrame *av_frame_ = nullptr;
+  AVFormatContext *pFormatCtx_ = nullptr;
+  AVCodecContext *pCodecCtx_ = nullptr;
+  int key_frames_count_ = 0;
+  std::vector<uint8_t> yuv_buf_;
+  bool valid_ = false;
+};
diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui
index 36341a7ee..68c8c2cad 100755
--- a/selfdrive/ui/ui
+++ b/selfdrive/ui/ui
@@ -1,5 +1,5 @@
 #!/bin/sh
 cd "$(dirname "$0")"
 export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH"
-export QT_PLUGIN_PATH="../../phonelibs/qt-plugins/$(uname -m)"
+export QT_PLUGIN_PATH="../../third_party/qt-plugins/$(uname -m)"
 exec ./_ui
diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
index f6fbb495e..e64606c62 100644
--- a/selfdrive/ui/ui.cc
+++ b/selfdrive/ui/ui.cc
@@ -6,13 +6,9 @@
 #include <cmath>
 #include <cstdio>
 
-#include "selfdrive/common/swaglog.h"
 #include "selfdrive/common/util.h"
-#include "selfdrive/common/visionimg.h"
 #include "selfdrive/common/watchdog.h"
 #include "selfdrive/hardware/hw.h"
-#include "selfdrive/ui/paint.h"
-#include "selfdrive/ui/qt/qt_window.h"
 
 #define BACKLIGHT_DT 0.05
 #define BACKLIGHT_TS 10.00
@@ -35,25 +31,6 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y,
   return out->x >= -margin && out->x <= s->fb_w + margin && out->y >= -margin && out->y <= s->fb_h + margin;
 }
 
-static void ui_init_vision(UIState *s) {
-  // Invisible until we receive a calibration message.
-  s->scene.world_objects_visible = false;
-
-  for (int i = 0; i < s->vipc_client->num_buffers; i++) {
-    s->texture[i].reset(new EGLImageTexture(&s->vipc_client->buffers[i]));
-
-    glBindTexture(GL_TEXTURE_2D, s->texture[i]->frame_tex);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-
-    // BGR
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_R, GL_BLUE);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_G, GL_GREEN);
-    glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
-  }
-  assert(glGetError() == GL_NO_ERROR);
-}
-
 static int get_path_length_idx(const cereal::ModelDataV2::XYZTData::Reader &line, const float path_height) {
   const auto line_x = line.getX();
   int max_idx = 0;
@@ -131,7 +108,8 @@ static void update_state(UIState *s) {
 
   // update engageability and DM icons at 2Hz
   if (sm.frame % (UI_FREQ / 2) == 0) {
-    scene.engageable = sm["controlsState"].getControlsState().getEngageable();
+    auto cs = sm["controlsState"].getControlsState();
+    scene.engageable = cs.getEngageable() || cs.getEnabled();
     scene.dm_active = sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode();
   }
   if (sm.updated("modelV2") && s->vg) {
@@ -156,11 +134,19 @@ static void update_state(UIState *s) {
       }
     }
   }
-  if (sm.updated("pandaState")) {
-    auto pandaState = sm["pandaState"].getPandaState();
-    scene.pandaType = pandaState.getPandaType();
-    scene.ignition = pandaState.getIgnitionLine() || pandaState.getIgnitionCan();
-  } else if ((s->sm->frame - s->sm->rcv_frame("pandaState")) > 5*UI_FREQ) {
+  if (sm.updated("pandaStates")) {
+    auto pandaStates = sm["pandaStates"].getPandaStates();
+    if (pandaStates.size() > 0) {
+      scene.pandaType = pandaStates[0].getPandaType();
+
+      if (scene.pandaType != cereal::PandaState::PandaType::UNKNOWN) {
+        scene.ignition = false;
+        for (const auto& pandaState : pandaStates) {
+          scene.ignition |= pandaState.getIgnitionLine() || pandaState.getIgnitionCan();
+        }
+      }
+    }
+  } else if ((s->sm->frame - s->sm->rcv_frame("pandaStates")) > 5*UI_FREQ) {
     scene.pandaType = cereal::PandaState::PandaType::UNKNOWN;
   }
   if (sm.updated("carParams")) {
@@ -207,25 +193,6 @@ static void update_params(UIState *s) {
   }
 }
 
-static void update_vision(UIState *s) {
-  if (!s->vipc_client->connected && s->scene.started) {
-    if (s->vipc_client->connect(false)) {
-      ui_init_vision(s);
-    }
-  }
-
-  if (s->vipc_client->connected) {
-    VisionBuf * buf = s->vipc_client->recv();
-    if (buf != nullptr) {
-      s->last_frame = buf;
-    } else if (!Hardware::PC()) {
-      LOGE("visionIPC receive timeout");
-    }
-  } else if (s->scene.started) {
-    util::sleep_for(1000. / UI_FREQ);
-  }
-}
-
 static void update_status(UIState *s) {
   if (s->scene.started && s->sm->updated("controlsState")) {
     auto controls_state = (*s->sm)["controlsState"].getControlsState();
@@ -245,24 +212,11 @@ static void update_status(UIState *s) {
     if (s->scene.started) {
       s->status = STATUS_DISENGAGED;
       s->scene.started_frame = s->sm->frame;
-
       s->scene.end_to_end = Params().getBool("EndToEndToggle");
       s->wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
-
-      // Update intrinsics matrix after possible wide camera toggle change
-      if (s->vg) {
-        ui_resize(s, s->fb_w, s->fb_h);
-      }
-
-      // Choose vision ipc client
-      if (s->wide_camera) {
-        s->vipc_client = s->vipc_client_wide;
-      } else {
-        s->vipc_client = s->vipc_client_rear;
-      }
-    } else {
-      s->vipc_client->connected = false;
     }
+    // Invisible until we receive a calibration message.
+    s->scene.world_objects_visible = false;
   }
   started_prev = s->scene.started;
 }
@@ -271,24 +225,15 @@ static void update_status(UIState *s) {
 QUIState::QUIState(QObject *parent) : QObject(parent) {
   ui_state.sm = std::make_unique<SubMaster, const std::initializer_list<const char *>>({
     "modelV2", "controlsState", "liveCalibration", "deviceState", "roadCameraState",
-    "pandaState", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
+    "pandaStates", "carParams", "driverMonitoringState", "sensorEvents", "carState", "liveLocationKalman",
   });
 
-  ui_state.fb_w = vwp_w;
-  ui_state.fb_h = vwp_h;
-  ui_state.scene.started = false;
-  ui_state.last_frame = nullptr;
   ui_state.wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
 
-  ui_state.vipc_client_rear = new VisionIpcClient("camerad", VISION_STREAM_RGB_BACK, true);
-  ui_state.vipc_client_wide = new VisionIpcClient("camerad", VISION_STREAM_RGB_WIDE, true);
-
-  ui_state.vipc_client = ui_state.vipc_client_rear;
-
   // update timer
   timer = new QTimer(this);
   QObject::connect(timer, &QTimer::timeout, this, &QUIState::update);
-  timer->start(0);
+  timer->start(1000 / UI_FREQ);
 }
 
 void QUIState::update() {
@@ -296,15 +241,10 @@ void QUIState::update() {
   update_sockets(&ui_state);
   update_state(&ui_state);
   update_status(&ui_state);
-  update_vision(&ui_state);
 
   if (ui_state.scene.started != started_prev || ui_state.sm->frame == 1) {
     started_prev = ui_state.scene.started;
     emit offroadTransition(!ui_state.scene.started);
-
-    // Change timeout to 0 when onroad, this will call update continuously.
-    // This puts visionIPC in charge of update frequency, reducing video latency
-    timer->start(ui_state.scene.started ? 0 : 1000 / UI_FREQ);
   }
 
   watchdog_kick();
diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h
index 7ad3b8b56..9930cb84a 100644
--- a/selfdrive/ui/ui.h
+++ b/selfdrive/ui/ui.h
@@ -1,6 +1,5 @@
 #pragma once
 
-#include <atomic>
 #include <map>
 #include <memory>
 #include <string>
@@ -12,16 +11,12 @@
 #include "nanovg.h"
 
 #include "cereal/messaging/messaging.h"
-#include "cereal/visionipc/visionipc.h"
-#include "cereal/visionipc/visionipc_client.h"
 #include "common/transformations/orientation.hpp"
 #include "selfdrive/camerad/cameras/camera_common.h"
-#include "selfdrive/common/glutil.h"
 #include "selfdrive/common/mat.h"
 #include "selfdrive/common/modeldata.h"
 #include "selfdrive/common/params.h"
 #include "selfdrive/common/util.h"
-#include "selfdrive/common/visionimg.h"
 
 #define COLOR_BLACK nvgRGBA(0, 0, 0, 255)
 #define COLOR_BLACK_ALPHA(x) nvgRGBA(0, 0, 0, x)
@@ -35,8 +30,8 @@ typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert;
 
 // TODO: this is also hardcoded in common/transformations/camera.py
 // TODO: choose based on frame input size
-const float y_offset = Hardware::TICI() ? 150.0 : 0.0;
-const float ZOOM = Hardware::TICI() ? 2912.8 : 2138.5;
+const float y_offset = Hardware::EON() ? 0.0 : 150.0;
+const float ZOOM = Hardware::EON() ? 2138.5 : 2912.8;
 
 typedef struct Rect {
   int x, y, w, h;
@@ -123,15 +118,7 @@ typedef struct UIScene {
 } UIScene;
 
 typedef struct UIState {
-  VisionIpcClient * vipc_client;
-  VisionIpcClient * vipc_client_rear;
-  VisionIpcClient * vipc_client_wide;
-  VisionBuf * last_frame;
-
-  // framebuffer
-  int fb_w, fb_h;
-
-  // NVG
+  int fb_w = 0, fb_h = 0;
   NVGcontext *vg;
 
   // images
@@ -140,14 +127,7 @@ typedef struct UIState {
   std::unique_ptr<SubMaster> sm;
 
   UIStatus status;
-  UIScene scene;
-
-  // graphics
-  std::unique_ptr<GLShader> gl_shader;
-  std::unique_ptr<EGLImageTexture> texture[UI_BUF_COUNT];
-
-  GLuint frame_vao, frame_vbo, frame_ibo;
-  mat4 rear_frame_mat;
+  UIScene scene = {};
 
   bool awake;
 
diff --git a/selfdrive/ui/watch3.cc b/selfdrive/ui/watch3.cc
new file mode 100644
index 000000000..e5e2864cc
--- /dev/null
+++ b/selfdrive/ui/watch3.cc
@@ -0,0 +1,28 @@
+#include <QApplication>
+#include <QtWidgets>
+
+#include "selfdrive/ui/qt/qt_window.h"
+#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/widgets/cameraview.h"
+
+int main(int argc, char *argv[]) {
+  QSurfaceFormat fmt;
+  fmt.setRenderableType(QSurfaceFormat::OpenGLES);
+  QSurfaceFormat::setDefaultFormat(fmt);
+
+  QApplication a(argc, argv);
+  QWidget w;
+  setMainWindow(&w);
+
+  QVBoxLayout *layout = new QVBoxLayout(&w);
+  layout->setMargin(0);
+  layout->setSpacing(0);
+  layout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_BACK, false));
+
+  QHBoxLayout *hlayout = new QHBoxLayout();
+  layout->addLayout(hlayout);
+  hlayout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_FRONT, false));
+  hlayout->addWidget(new CameraViewWidget(VISION_STREAM_RGB_WIDE, false));
+
+  return a.exec();
+}
diff --git a/selfdrive/updated.py b/selfdrive/updated.py
index cbc2c387d..a74c8c799 100755
--- a/selfdrive/updated.py
+++ b/selfdrive/updated.py
@@ -216,11 +216,14 @@ def finalize_update() -> None:
     shutil.rmtree(FINALIZED)
   shutil.copytree(OVERLAY_MERGED, FINALIZED, symlinks=True)
 
+  run(["git", "reset", "--hard"], FINALIZED)
+  run(["git", "submodule", "foreach", "--recursive", "git", "reset"], FINALIZED)
+
   set_consistent_flag(True)
   cloudlog.info("done finalizing overlay")
 
 
-def handle_agnos_update(wait_helper):
+def handle_agnos_update(wait_helper: WaitTimeHelper) -> None:
   from selfdrive.hardware.tici.agnos import flash_agnos_update, get_target_slot_number
 
   cur_version = HARDWARE.get_os_version()
@@ -244,6 +247,8 @@ def handle_agnos_update(wait_helper):
 
 
 def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
+  from selfdrive.hardware.eon.neos import download_neos_update
+
   cur_neos = HARDWARE.get_os_version()
   updated_neos = run(["bash", "-c", r"unset REQUIRED_NEOS_VERSION && source launch_env.sh && \
                        echo -n $REQUIRED_NEOS_VERSION"], OVERLAY_MERGED).strip()
@@ -255,8 +260,7 @@ def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
   cloudlog.info(f"Beginning background download for NEOS {updated_neos}")
   set_offroad_alert("Offroad_NeosUpdate", True)
 
-  updater_path = os.path.join(OVERLAY_MERGED, "installer/updater/updater")
-  update_manifest = f"file://{OVERLAY_MERGED}/installer/updater/update.json"
+  update_manifest = os.path.join(OVERLAY_MERGED, "selfdrive/hardware/eon/neos.json")
 
   neos_downloaded = False
   start_time = time.monotonic()
@@ -265,9 +269,9 @@ def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
         (time.monotonic() - start_time < 60*60*24):
     wait_helper.ready_event.clear()
     try:
-      run([updater_path, "bgcache", update_manifest], OVERLAY_MERGED, low_priority=True)
+      download_neos_update(update_manifest, cloudlog)
       neos_downloaded = True
-    except subprocess.CalledProcessError:
+    except Exception:
       cloudlog.info("NEOS background download failed, retrying")
       wait_helper.sleep(120)
 
@@ -278,7 +282,7 @@ def handle_neos_update(wait_helper: WaitTimeHelper) -> None:
   cloudlog.info(f"NEOS background download successful, took {time.monotonic() - start_time} seconds")
 
 
-def check_git_fetch_result(fetch_txt):
+def check_git_fetch_result(fetch_txt: str) -> bool:
   err_msg = "Failed to add the host to the list of known hosts (/data/data/com.termux/files/home/.ssh/known_hosts).\n"
   return len(fetch_txt) > 0 and (fetch_txt != err_msg)
 
@@ -342,6 +346,12 @@ def main():
   if EON and os.geteuid() != 0:
     raise RuntimeError("updated must be launched as root!")
 
+  ov_lock_fd = open(LOCK_FILE, 'w')
+  try:
+    fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
+  except IOError as e:
+    raise RuntimeError("couldn't get overlay lock; is another updated running?") from e
+
   # Set low io priority
   proc = psutil.Process()
   if psutil.LINUX:
@@ -351,11 +361,9 @@ def main():
   if Path(os.path.join(STAGING_ROOT, "old_openpilot")).is_dir():
     cloudlog.event("update installed")
 
-  ov_lock_fd = open(LOCK_FILE, 'w')
-  try:
-    fcntl.flock(ov_lock_fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
-  except IOError as e:
-    raise RuntimeError("couldn't get overlay lock; is another updated running?") from e
+  if not params.get("InstallDate"):
+    t = datetime.datetime.utcnow().isoformat()
+    params.put("InstallDate", t.encode('utf8'))
 
   # Wait for IsOffroad to be set before our first update attempt
   wait_helper = WaitTimeHelper(proc)
diff --git a/selfdrive/version.py b/selfdrive/version.py
index d721ba61e..7a2234e3e 100644
--- a/selfdrive/version.py
+++ b/selfdrive/version.py
@@ -94,6 +94,12 @@ if (origin is not None) and (branch is not None):
 
 
 if __name__ == "__main__":
+  from common.params import Params
+
+  params = Params()
+  params.put("TermsVersion", terms_version)
+  params.put("TrainingVersion", training_version)
+
   print("Dirty: %s" % dirty)
   print("Version: %s" % version)
   print("Remote: %s" % origin)
diff --git a/phonelibs/SConscript b/third_party/SConscript
similarity index 100%
rename from phonelibs/SConscript
rename to third_party/SConscript
diff --git a/third_party/acados/aarch64/lib/libacados.so b/third_party/acados/aarch64/lib/libacados.so
new file mode 100755
index 000000000..248e8c8fa
Binary files /dev/null and b/third_party/acados/aarch64/lib/libacados.so differ
diff --git a/third_party/acados/aarch64/lib/libblasfeo.so b/third_party/acados/aarch64/lib/libblasfeo.so
new file mode 100755
index 000000000..38cfb0e9e
Binary files /dev/null and b/third_party/acados/aarch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/aarch64/lib/libhpipm.so b/third_party/acados/aarch64/lib/libhpipm.so
new file mode 100755
index 000000000..fa7eae088
Binary files /dev/null and b/third_party/acados/aarch64/lib/libhpipm.so differ
diff --git a/third_party/acados/aarch64/lib/libqpOASES_e.so b/third_party/acados/aarch64/lib/libqpOASES_e.so
new file mode 120000
index 000000000..14048625a
--- /dev/null
+++ b/third_party/acados/aarch64/lib/libqpOASES_e.so
@@ -0,0 +1 @@
+libqpOASES_e.so.3.1
\ No newline at end of file
diff --git a/third_party/acados/aarch64/lib/libqpOASES_e.so.3.1 b/third_party/acados/aarch64/lib/libqpOASES_e.so.3.1
new file mode 100755
index 000000000..98d8dbfcd
Binary files /dev/null and b/third_party/acados/aarch64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/aarch64/t_renderer b/third_party/acados/aarch64/t_renderer
new file mode 100755
index 000000000..dc3cdf554
Binary files /dev/null and b/third_party/acados/aarch64/t_renderer differ
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_common.h b/third_party/acados/include/acados/dense_qp/dense_qp_common.h
new file mode 100644
index 000000000..f3809c429
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_common.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
+#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_res.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+// acados
+#include "acados/utils/types.h"
+
+typedef struct d_dense_qp_dim dense_qp_dims;
+typedef struct d_dense_qp dense_qp_in;
+typedef struct d_dense_qp_sol dense_qp_out;
+typedef struct d_dense_qp_res dense_qp_res;
+typedef struct d_dense_qp_res_ws dense_qp_res_ws;
+
+
+
+#ifndef QP_SOLVER_CONFIG_
+#define QP_SOLVER_CONFIG_
+typedef struct
+{
+    void (*dims_set)(void *config_, void *dims_, const char *field, const int* value);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *args);
+    void (*opts_update)(void *config, void *dims, void *args);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args);
+    void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory);
+    void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args);
+    int (*evaluate)(void *config, void *qp_in, void *qp_out, void *args, void *mem, void *work);
+    void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+} qp_solver_config;
+#endif
+
+
+
+#ifndef QP_INFO_
+#define QP_INFO_
+typedef struct
+{
+    double solve_QP_time;
+    double condensing_time;
+    double interface_time;
+    double total_time;
+    int num_iter;
+    int t_computed;
+} qp_info;
+#endif
+
+
+
+/* config */
+//
+acados_size_t dense_qp_solver_config_calculate_size();
+//
+qp_solver_config *dense_qp_solver_config_assign(void *raw_memory);
+
+/* dims */
+//
+acados_size_t dense_qp_dims_calculate_size();
+//
+dense_qp_dims *dense_qp_dims_assign(void *raw_memory);
+//
+void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value);
+//
+
+/* in */
+//
+acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory);
+
+/* out */
+//
+acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_out_get(dense_qp_out *out, const char *field, void *value);
+
+/* res */
+//
+acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory);
+//
+acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out);
+//
+void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws);
+//
+void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]);
+
+/* misc */
+//
+void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out);
+//
+void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out);
+//
+void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
new file mode 100644
index 000000000..20eedc26a
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
+#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+
+
+typedef struct dense_qp_hpipm_opts_
+{
+    struct d_dense_qp_ipm_arg *hpipm_opts;
+} dense_qp_hpipm_opts;
+
+
+
+typedef struct dense_qp_hpipm_memory_
+{
+    struct d_dense_qp_ipm_ws *hpipm_workspace;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_hpipm_memory;
+
+
+
+//
+acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims);
+//
+void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
+//
+acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_);
+//
+void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_);
+//
+int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_hpipm_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
new file mode 100644
index 000000000..646f11f06
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h
@@ -0,0 +1,128 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
+#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+enum dense_qp_ooqp_termination_code
+{
+  DENSE_SUCCESSFUL_TERMINATION = 0,
+  DENSE_NOT_FINISHED,
+  DENSE_MAX_ITS_EXCEEDED,
+  DENSE_INFEASIBLE,
+  DENSE_UNKNOWN
+};
+
+typedef struct dense_qp_ooqp_opts_
+{
+    int printLevel;
+    int useDiagonalWeights;  // TODO(dimitris): implement option
+    int fixHessian;
+    int fixDynamics;
+    int fixInequalities;
+} dense_qp_ooqp_opts;
+
+typedef struct dense_qp_ooqp_workspace_
+{
+    double *x;
+    double *gamma;
+    double *phi;
+    double *y;
+    double *z;
+    double *lambda;
+    double *pi;
+    double objectiveValue;
+} dense_qp_ooqp_workspace;
+
+typedef struct dense_qp_ooqp_memory_
+{
+    int firstRun;
+    int nx;
+    int my;
+    int mz;
+    double *c;
+    double *dQ;
+    double *xlow;
+    char *ixlow;
+    double *xupp;
+    char *ixupp;
+    double *dA;
+    double *bA;
+    double *dC;
+    double *clow;
+    char *iclow;
+    double *cupp;
+    char *icupp;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_ooqp_memory;
+
+//
+acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims);
+//
+void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
+                                  void *raw_memory);
+//
+acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
+                  void *memory_, void *work_);
+//
+void dense_qp_ooqp_destroy(void *mem_, void *work);
+//
+void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_ooqp_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h
new file mode 100644
index 000000000..52606fac5
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_QORE_H_
+#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// qore
+#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef struct dense_qp_qore_opts_
+{
+    int nsmax;       // maximum size of Schur complement
+    int print_freq;  // print frequency,
+                     // prtfreq  < 0: disable printing;
+                     // prtfreq == 0: print on each call and include working set changes;
+                     // prtfreq  > 0: print on every prtfreq seconds, but do not include working set
+                     // changes;
+    int warm_start;  // warm start with updated matrices H and C
+    int warm_strategy;  // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous
+                        // solution
+    int hot_start;      // hot start with unchanged matrices H and C
+    int max_iter;       // maximum number of iterations
+    int compute_t;      // compute t in qp_out (to have correct residuals in NLP)
+} dense_qp_qore_opts;
+
+typedef struct dense_qp_qore_memory_
+{
+    double *H;
+    double *HH;
+    double *g;
+    double *gg;
+    double *Zl;
+    double *Zu;
+    double *zl;
+    double *zu;
+    double *A;
+    double *b;
+    double *C;
+    double *CC;
+    double *Ct;
+    double *CCt;
+    double *d_lb0;
+    double *d_ub0;
+    double *d_lb;
+    double *d_ub;
+    double *d_lg;
+    double *d_ug;
+    double *d_ls;
+    double *d_us;
+    double *lb;
+    double *ub;
+    int *idxb;
+    int *idxb_stacked;
+    int *idxs;
+    double *prim_sol;
+    double *dual_sol;
+    QoreProblemDense *QP;
+    int num_iter;
+    dense_qp_in *qp_stacked;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_qore_memory;
+
+acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_qore_config_initialize_default(void *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_QORE_H_
diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
new file mode 100644
index 000000000..8fca17dd1
--- /dev/null
+++ b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h
@@ -0,0 +1,127 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
+#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef struct dense_qp_qpoases_opts_
+{
+    double max_cputime;  // maximum cpu time in seconds
+    int max_nwsr;        // maximum number of working set recalculations
+    int warm_start;      // warm start with dual_sol in memory
+    int use_precomputed_cholesky;
+    int hotstart;  // this option requires constant data matrices! (eg linear MPC, inexact schemes
+                   // with frozen sensitivities)
+    int set_acado_opts;  // use same options as in acado code generation
+    int compute_t;       // compute t in qp_out (to have correct residuals in NLP)
+    double tolerance;  // terminationTolerance
+} dense_qp_qpoases_opts;
+
+typedef struct dense_qp_qpoases_memory_
+{
+    double *H;
+    double *HH;
+    double *R;
+    double *g;
+    double *gg;
+    double *Zl;
+    double *Zu;
+    double *zl;
+    double *zu;
+    double *A;
+    double *b;
+    double *d_lb0;
+    double *d_ub0;
+    double *d_lb;
+    double *d_ub;
+    double *C;
+    double *CC;
+    double *d_lg0;
+    double *d_ug0;
+    double *d_lg;
+    double *d_ug;
+    double *d_ls;
+    double *d_us;
+    int *idxb;
+    int *idxb_stacked;
+    int *idxs;
+    double *prim_sol;
+    double *dual_sol;
+    void *QPB;       // NOTE(giaf): cast to QProblemB to use
+    void *QP;        // NOTE(giaf): cast to QProblem to use
+    double cputime;  // cputime of qpoases
+    int nwsr;        // performed number of working set recalculations
+    int first_it;    // to be used with hotstart
+    dense_qp_in *qp_stacked;
+    double time_qp_solver_call; // equal to cputime
+    int iter;
+
+} dense_qp_qpoases_memory;
+
+acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_qpoases_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
new file mode 100644
index 000000000..040763381
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
@@ -0,0 +1,427 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+
+/// \defgroup ocp_nlp ocp_nlp
+/// @{
+/// @}
+
+/// \defgroup ocp_nlp_solver ocp_nlp_solver
+/// @{
+/// @}
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \ingroup ocp_nlp_solver
+/// @{
+
+/// \defgroup ocp_nlp_common ocp_nlp_common
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct ocp_nlp_config
+{
+    int N;  // number of stages
+
+    // solver-specific implementations of memory management functions
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts_);
+    void (*opts_update)(void *config, void *dims, void *opts_);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_);
+    void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
+    // evaluate solver // TODO rename into solve
+    int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
+                            char *field, int stage, int index, void *sens_nlp_out);
+    // prepare memory
+    int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    // initalize this struct with default values
+    void (*config_initialize_default)(void *config);
+    // general getter
+    void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_);
+    void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_);
+    void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_);
+    // config structs of submodules
+    ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver
+    ocp_nlp_dynamics_config **dynamics;
+    ocp_nlp_cost_config **cost;
+    ocp_nlp_constraints_config **constraints;
+    ocp_nlp_reg_config *regularize;
+
+} ocp_nlp_config;
+
+//
+acados_size_t ocp_nlp_config_calculate_size(int N);
+//
+ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory);
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+/// Structure to store dimensions/number of variables.
+typedef struct ocp_nlp_dims
+{
+    void **cost;
+    void **dynamics;
+    void **constraints;
+    ocp_qp_xcond_solver_dims *qp_solver;  // xcond solver instead ??
+    ocp_nlp_reg_dims *regularize;
+
+    int *nv;  // number of primal variables (states+controls+slacks)
+    int *nx;  // number of differential states
+    int *nu;  // number of inputs
+    int *ni;  // number of two-sided inequality constraints: nb+ng+nh+ns
+    int *nz;  // number of algebraic variables
+    int *ns;  // number of slack variables
+    int N;    // number of shooting nodes
+} ocp_nlp_dims;
+
+//
+acados_size_t ocp_nlp_dims_calculate_size(void *config);
+//
+ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory);
+
+/// Sets the dimension of optimization variables
+/// (states, constrols, algebraic variables, slack variables).
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param field The type of optimization variables, either nx, nu, nz, or ns.
+/// \param value_array Number of variables for each stage.
+void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_,
+                               const char *field, const void* value_array);
+
+/// Sets the dimensions of constraints functions for a stage
+/// (bounds on states, bounds on controls, equality constraints,
+/// inequality constraints).
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh.
+/// \param value_field Number of constraints/bounds for the given stage.
+void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage,
+                                  const char *field, const void* value_field);
+
+/// Sets the dimensions of the cost terms for a stage.
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field Type of cost term, can be eiter ny.
+/// \param value_field Number of cost terms/residuals for the given stage.
+void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field,
+                           const void* value_field);
+
+/// Sets the dimensions of the dynamics for a stage.
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field TBD
+/// \param value TBD
+void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field,
+                               const void* value);
+
+/************************************************
+ * Inputs
+ ************************************************/
+
+/// Struct for storing the inputs of an OCP NLP solver
+typedef struct ocp_nlp_in
+{
+    /// Length of sampling intervals/timesteps.
+    double *Ts;
+
+    /// Pointers to cost functions (TBC).
+    void **cost;
+
+    /// Pointers to dynamics functions (TBC).
+    void **dynamics;
+
+    /// Pointers to constraints functions (TBC).
+    void **constraints;
+
+} ocp_nlp_in;
+
+//
+acados_size_t ocp_nlp_in_calculate_size_self(int N);
+//
+acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
+//
+ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory);
+//
+ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory);
+
+
+/************************************************
+ * out
+ ************************************************/
+
+typedef struct ocp_nlp_out
+{
+    struct blasfeo_dvec *ux;  // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs?
+    struct blasfeo_dvec *z;  // algebraic vairables
+    struct blasfeo_dvec *pi;  // multipliers for dynamics
+    struct blasfeo_dvec *lam;  // inequality mulitpliers
+    struct blasfeo_dvec *t;  // slack variables corresponding to evaluation of all inequalities (at the solution)
+
+    // NOTE: the inequalities are internally organized in the following order:
+    // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+
+    int sqp_iter;
+    int qp_iter;
+    double inf_norm_res;
+    double total_time;
+
+} ocp_nlp_out;
+
+//
+acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
+//
+ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                void *raw_memory);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+/// Globalization types
+typedef enum
+{
+    FIXED_STEP,
+    MERIT_BACKTRACKING,
+} ocp_nlp_globalization_t;
+
+typedef struct ocp_nlp_opts
+{
+    ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ???
+    void *regularize;
+    void **dynamics;     // dynamics_opts
+    void **cost;         // cost_opts
+    void **constraints;  // constraints_opts
+    double step_length;  // step length in case of FIXED_STEP
+    double levenberg_marquardt;  // LM factor to be added to the hessian before regularization
+    int reuse_workspace;
+    int num_threads;
+
+    // TODO: move to separate struct?
+    ocp_nlp_globalization_t globalization;
+    double alpha_min;
+    double alpha_reduction;
+} ocp_nlp_opts;
+
+//
+acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value);
+
+
+/************************************************
+ * residuals
+ ************************************************/
+
+typedef struct ocp_nlp_res
+{
+    struct blasfeo_dvec *res_stat;  // stationarity
+    struct blasfeo_dvec *res_eq;  // dynamics
+    struct blasfeo_dvec *res_ineq;  // inequality constraints
+    struct blasfeo_dvec *res_comp;  // complementarity
+    double inf_norm_res_stat;
+    double inf_norm_res_eq;
+    double inf_norm_res_ineq;
+    double inf_norm_res_comp;
+    acados_size_t memsize;
+} ocp_nlp_res;
+
+//
+acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
+//
+ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct ocp_nlp_memory
+{
+//    void *qp_solver_mem; // xcond solver mem instead ???
+    ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ???
+    void *regularize_mem;
+    void **dynamics;     // dynamics memory
+    void **cost;         // cost memory
+    void **constraints;  // constraints memory
+
+    // residuals
+    ocp_nlp_res *nlp_res;
+
+    // qp in & out
+    ocp_qp_in *qp_in;
+    ocp_qp_out *qp_out;
+    // QP stuff not entering the qp_in struct
+    struct blasfeo_dmat *dzduxt; // dzdux transposed
+    struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables
+
+    struct blasfeo_dvec *cost_grad;
+    struct blasfeo_dvec *ineq_fun;
+    struct blasfeo_dvec *ineq_adj;
+    struct blasfeo_dvec *dyn_fun;
+    struct blasfeo_dvec *dyn_adj;
+
+    double cost_value;
+
+    bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
+    struct blasfeo_dvec *sim_guess;
+
+	int *sqp_iter; // pointer to iteration number
+
+} ocp_nlp_memory;
+
+//
+acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
+//
+ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                      ocp_nlp_opts *opts, void *raw_memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct ocp_nlp_workspace
+{
+
+    void *qp_work;
+    void **dynamics;     // dynamics_workspace
+    void **cost;         // cost_workspace
+    void **constraints;  // constraints_workspace
+
+	ocp_nlp_out *tmp_nlp_out;
+	ocp_nlp_out *weight_merit_fun;
+
+} ocp_nlp_workspace;
+
+//
+acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts);
+//
+ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory);
+
+
+
+/************************************************
+ * function
+ ************************************************/
+
+//
+void ocp_nlp_initialize_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+                 ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+                 ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+           ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha);
+//
+double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+          ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out,
+                         ocp_nlp_res *res, ocp_nlp_memory *mem);
+//
+void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
new file mode 100644
index 000000000..7f7a30faf
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h
@@ -0,0 +1,241 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+    int nb;  // nbx + nbu
+    int nbu; // number of input box constraints
+    int nbx; // number of state box constraints
+    int ng;  // number of general linear constraints
+    int nh;  // number of nonlinear path constraints
+    int ns;  // nsbu + nsbx + nsg + nsh
+    int nsbu;  // number of softened input bounds
+    int nsbx;  // number of softened state bounds
+    int nsg;  // number of softened general linear constraints
+    int nsh;  // number of softened nonlinear constraints
+    int nbue; // number of input box constraints which are equality
+    int nbxe; // number of state box constraints which are equality
+    int nge;  // number of general linear constraints which are equality
+    int nhe;  // number of nonlinear path constraints which are equality
+} ocp_nlp_constraints_bgh_dims;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
+//
+void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_constraints_bgh_dims_initialize(void *config, void *dims, int nx, int nu, int nz, int nbx,
+                                         int nbu, int ng, int nh, int dummy0, int ns);
+//
+void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
+                                      const char *field, const int* value);
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    int *idxb;
+    int *idxs;
+    int *idxe;
+    struct blasfeo_dvec d;  // gathers bounds
+    struct blasfeo_dmat DCt;  // general linear constraint matrix
+            // lg <= [D, C] * [u; x] <= ug
+    external_function_generic *nl_constr_h_fun;  // nonlinear: lh <= h(x,u) <= uh
+    external_function_generic *nl_constr_h_fun_jac;  // nonlinear: lh <= h(x,u) <= uh
+    external_function_generic *nl_constr_h_fun_jac_hess;  // nonlinear: lh <= h(x,u) <= uh
+} ocp_nlp_constraints_bgh_model;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_constraints_bgh_opts;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
+    struct blasfeo_dvec *lam;    // pointer to lam in nlp_out
+    struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
+    struct blasfeo_dvec *z_alg;  // pointer to z_alg in ocp_nlp memory
+    struct blasfeo_dmat *DCt;    // pointer to DCt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
+    int *idxb;                   // pointer to idxb[ii] in qp_in
+    int *idxs_rev;               // pointer to idxs_rev[ii] in qp_in
+    int *idxe;                   // pointer to idxe[ii] in qp_in
+} ocp_nlp_constraints_bgh_memory;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
+//
+void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dmat tmp_nz_nh;
+    struct blasfeo_dmat tmp_nv_nh;
+    struct blasfeo_dmat tmp_nz_nv;
+    struct blasfeo_dmat hess_z;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec tmp_nh;
+} ocp_nlp_constraints_bgh_workspace;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/* functions */
+
+//
+void ocp_nlp_constraints_bgh_config_initialize_default(void *config);
+//
+void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts,
+                                    void *mem, void *work);
+//
+void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+
+//
+void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
new file mode 100644
index 000000000..beeec7841
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/* dims */
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+    int nb;   // nbx + nbu
+    int nbu;
+    int nbx;
+    int ng;   // number of general linear constraints
+    int nphi; // dimension of convex outer part 
+    int ns;   // nsbu + nsbx + nsg + nsphi
+    int nsbu; // number of softened input bounds
+    int nsbx; // number of softened state bounds
+    int nsg;  // number of softened general linear constraints
+    int nsphi;  // number of softened nonlinear constraints
+    int nr;   // dimension of nonlinear function in convex_over_nonlinear constraint
+    int nbue; // number of input box constraints which are equality
+    int nbxe; // number of state box constraints which are equality
+    int nge;  // number of general linear constraints which are equality
+    int nphie;  // number of nonlinear path constraints which are equality
+} ocp_nlp_constraints_bgp_dims;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
+//
+void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_constraints_bgp_dims_initialize(void *config, void *dims, int nx, int nu, int nz, 
+		int nbx, int nbu, int ng, int nphi, int nq, int ns);
+//
+void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+/* model */
+
+typedef struct
+{
+    //  ocp_nlp_constraints_bgp_dims *dims;
+    int *idxb;
+    int *idxs;
+    int *idxe;
+    struct blasfeo_dvec d;
+    struct blasfeo_dmat DCt;
+    external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux;
+    external_function_generic *nl_constr_phi_o_r_fun;
+    external_function_generic *nl_constr_r_fun_jac;
+} ocp_nlp_constraints_bgp_model;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+
+/* options */
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_constraints_bgp_opts;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value);
+
+/* memory */
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out
+    struct blasfeo_dvec *lam;    // pointer to lam in nlp_out
+    struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out
+    struct blasfeo_dvec *z_alg;  // pointer to z_alg in ocp_nlp memory
+    struct blasfeo_dmat *DCt;    // pointer to DCt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
+    int *idxb;                   // pointer to idxb[ii] in qp_in
+    int *idxs_rev;                   // pointer to idxs_rev[ii] in qp_in
+    int *idxe;                   // pointer to idxe[ii] in qp_in
+} ocp_nlp_constraints_bgp_memory;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts,
+    void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
+//
+void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
+
+/* workspace */
+
+typedef struct
+{
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dmat jac_r_ux_tran;
+    struct blasfeo_dmat tmp_nr_nphi_nr;
+    struct blasfeo_dmat tmp_nv_nr;
+    struct blasfeo_dmat tmp_nv_nphi;
+    struct blasfeo_dmat tmp_nz_nphi;
+} ocp_nlp_constraints_bgp_workspace;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/* functions */
+
+//
+void ocp_nlp_constraints_bgp_config_initialize_default(void *config);
+//
+void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model,
+        void *opts, void *mem, void *work);
+//
+void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims,
+        void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims,
+        void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_,
+        void *opts_, void *memory_, void *work_);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
new file mode 100644
index 000000000..7cadecab4
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_constraints ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nz, int nbx, int nbu, int ng,
+                            int nh, int nq, int ns);
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, char *field, void *value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory);
+    struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
+    void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
+    void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory);
+    void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory);
+    void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
+    void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory);
+    void (*memory_set_idxb_ptr)(int *idxb, void *memory);
+    void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory);
+    void (*memory_set_idxe_ptr)(int *idxe, void *memory);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*config_initialize_default)(void *config);
+    // dimension setters
+    void (*dims_set)(void *config_, void *dims_, const char *field, const int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
+} ocp_nlp_constraints_config;
+
+//
+acados_size_t ocp_nlp_constraints_config_calculate_size();
+//
+ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
new file mode 100644
index 000000000..c9fbbfb40
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+///
+/// \defgroup ocp_nlp_cost ocp_nlp_cost 
+/// 
+
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
+    void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, const char *field, void *value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+	double *(*memory_get_fun_ptr)(void *memory);
+    struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
+    void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
+    void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
+    void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+
+    // computes the function value, gradient and hessian (approximation) of the cost function
+    void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    // computes the cost function value (intended for globalization)
+    void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*config_initialize_default)(void *config);
+} ocp_nlp_cost_config;
+
+//
+acados_size_t ocp_nlp_cost_config_calculate_size();
+//
+ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+/// @} 
+/// @} 
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
new file mode 100644
index 000000000..f2196dbee
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h
@@ -0,0 +1,187 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nu;  // number of inputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_external_dims;
+
+//
+acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx,
+                                           int nu, int ny, int ns, int nz);
+//
+void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    external_function_generic *ext_cost_fun;  // function
+    external_function_generic *ext_cost_fun_jac_hess;  // function, gradient and hessian
+    external_function_generic *ext_cost_fun_jac;  // function, gradient
+    struct blasfeo_dvec Z;
+    struct blasfeo_dvec z;
+    struct blasfeo_dmat numerical_hessian;  // custom hessian approximation
+    double scaling;
+} ocp_nlp_cost_external_model;
+
+//
+acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int use_numerical_hessian;  // > 0 indicating custom hessian is used instead of CasADi evaluation
+} ocp_nlp_cost_external_opts;
+
+//
+acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux;     // pointer to tmp_ux in nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+	double fun;                         ///< value of the cost function
+} ocp_nlp_cost_external_memory;
+
+//
+acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dvec tmp_2ns;  // temporary vector of dimension 2*ns
+} ocp_nlp_cost_external_workspace;
+
+//
+acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_external_config_initialize_default(void *config);
+//
+void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_,
+                                      void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_,
+                                               void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_,
+                                       void *opts_, void *memory_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
new file mode 100644
index 000000000..3cf759504
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h
@@ -0,0 +1,259 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls
+/// \brief This module implements linear-least squares costs of the form
+/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$.
+/// @{
+
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     dims                                   //
+////////////////////////////////////////////////////////////////////////////////
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_ls_dims;
+
+
+///  Calculate the size of the ocp_nlp_cost_ls_dims struct
+///
+///  \param[in] config_ structure containing configuration of ocp_nlp_cost
+///  module
+///  \param[out] []
+///  \return \c size of ocp_nlp_dims struct
+acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
+
+
+///  Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct 
+///
+///  \param[in] config structure containing configuration of ocp_nlp_cost 
+///  module
+///  \param[in] raw_memory pointer to memory location  
+///  \param[out] []
+///  \return dims
+void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
+
+
+///  Initialize the dimensions struct of the 
+///  ocp_nlp-cost_ls component    
+///
+///  \param[in] config structure containing configuration ocp_nlp-cost_ls component 
+///  \param[in] nx number of states
+///  \param[in] nu number of inputs
+///  \param[in] ny number of residuals
+///  \param[in] ns number of slacks
+///  \param[in] nz number of algebraic variables
+///  \param[out] dims
+///  \return size
+void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx,
+        int nu, int ny, int ns, int nz);
+
+//
+void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     model                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+/// structure containing the data describing the linear least-square cost 
+typedef struct
+{
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    struct blasfeo_dmat Cyt;            ///< output matrix: Cy * [u,x] = y; in transposed form
+    struct blasfeo_dmat Vz;             ///< Vz in ls cost Vx*x + Vu*u + Vz*z
+    struct blasfeo_dmat W;              ///< ls norm corresponding to this matrix
+    struct blasfeo_dvec y_ref;          ///< yref
+    struct blasfeo_dvec Z;              ///< diagonal Hessian of slacks as vector (lower and upper)
+    struct blasfeo_dvec z;              ///< gradient of slacks as vector (lower and upper)
+    double scaling;
+} ocp_nlp_cost_ls_model;
+
+//
+acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_,
+                              const char *field, void *value_);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                   options                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+typedef struct
+{
+    int dummy; // struct can't be void
+} ocp_nlp_cost_ls_opts;
+
+//
+acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     memory                                 //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+/// structure containing the memory associated with cost_ls component 
+/// of the ocp_nlp module
+typedef struct
+{
+    struct blasfeo_dmat hess;           ///< hessian of cost function
+    struct blasfeo_dmat W_chol;         ///< cholesky factor of weight matrix
+    struct blasfeo_dvec res;            ///< ls residual r(x)
+    struct blasfeo_dvec grad;           ///< gradient of cost function
+    struct blasfeo_dvec *ux;            ///< pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux;        ///< pointer to ux in tmp_nlp_out
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    struct blasfeo_dmat *RSQrq;         ///< pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;             ///< pointer to Z in qp_in
+	double fun;                         ///< value of the cost function
+} ocp_nlp_cost_ls_memory;
+
+//
+acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                 workspace                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_ny;   // temporary matrix of dimensions nv, ny
+    struct blasfeo_dmat Cyt_tilde;   // updated Cyt (after z elimination)
+    struct blasfeo_dmat dzdux_tran;  // derivatives of z wrt u and x (tran)
+    struct blasfeo_dvec tmp_ny;      // temporary vector of dimension ny
+    struct blasfeo_dvec tmp_2ns;     // temporary vector of dimension ny
+    struct blasfeo_dvec tmp_nz;      // temporary vector of dimension nz
+    struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination)
+} ocp_nlp_cost_ls_workspace;
+
+//
+acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                 functions                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+//
+void ocp_nlp_cost_ls_config_initialize_default(void *config);
+//
+void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_,
+                                void *mem_, void *work_);
+//
+void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_,
+                                        void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
new file mode 100644
index 000000000..aafb6b354
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h
@@ -0,0 +1,210 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
+/// \brief This module implements nonlinear-least squares costs of the form
+/// \f$\min_{x,u} \| r(x,u) - y_{\text{ref}} \|_W^2\f$.
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_nls_dims;
+
+//
+acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
+//
+void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    // nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    external_function_generic *nls_y_fun;  // evaluation of nls function
+    external_function_generic *nls_y_fun_jac;  // evaluation nls function and jacobian
+    external_function_generic *nls_y_hess;  // hessian*seeds of nls residuals
+    struct blasfeo_dmat W;                //
+    struct blasfeo_dvec y_ref;
+    struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
+    struct blasfeo_dvec z;              // gradient of slacks as vector
+    double scaling;
+} ocp_nlp_cost_nls_model;
+
+//
+acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    bool gauss_newton_hess;  // gauss-newton hessian approximation
+} ocp_nlp_cost_nls_opts;
+
+//
+acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat W_chol;  // cholesky factor of weight matrix
+    struct blasfeo_dmat Jt;      // jacobian of nls fun
+    struct blasfeo_dvec res;     // nls residual r(x)
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *tmp_ux;     // pointer to ux in tmp_nlp_out
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+	double fun;                         ///< value of the cost function
+} ocp_nlp_cost_nls_memory;
+
+//
+acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_ny;
+    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;     // temporary vector of dimension ny
+} ocp_nlp_cost_nls_workspace;
+
+//
+acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts);
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_nls_config_initialize_default(void *config);
+//
+void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
new file mode 100644
index 000000000..45dac20a2
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h
@@ -0,0 +1,122 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    void (*config_initialize_default)(void *config);
+    sim_config *sim_solver;
+    /* dims */
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_initialize)(void *config, void *dims, int nx, int nu, int nx1, int nu1, int nz);
+    void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
+    /* model */
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
+    /* opts */
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    /* memory */
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
+    struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
+    void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_);
+    void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_);
+    void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_);
+    void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_);
+    void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_);
+    void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_);
+    void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_);
+    void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_);
+    void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value);
+    /* workspace */
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+} ocp_nlp_dynamics_config;
+
+//
+acados_size_t ocp_nlp_dynamics_config_calculate_size();
+//
+ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
new file mode 100644
index 000000000..59a2df4f4
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h
@@ -0,0 +1,216 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+#include "acados_c/sim_interface.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    void *sim;
+    int nx;   // number of states at the current stage
+    int nz;   // number of algebraic states at the current stage
+    int nu;   // number of inputs at the current stage
+    int nx1;  // number of states at the next stage
+    int nu1;  // number of inputes at the next stage
+} ocp_nlp_dynamics_cont_dims;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
+//
+void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_dims_initialize(void *config, void *dims, int nx, int nu, int nx1,
+                                           int nu1, int nz);
+
+//
+void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    void *sim_solver;
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_dynamics_cont_opts;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;            // pointer to ux in nlp_out at current stage
+    struct blasfeo_dvec *tmp_ux;        // pointer to ux in tmp_nlp_out at current stage
+    struct blasfeo_dvec *ux1;           // pointer to ux in nlp_out at next stage
+    struct blasfeo_dvec *tmp_ux1;       // pointer to ux in tmp_nlp_out at next stage
+    struct blasfeo_dvec *pi;            // pointer to pi in nlp_out at current stage
+    struct blasfeo_dvec *tmp_pi;        // pointer to pi in tmp_nlp_out at current stage
+    struct blasfeo_dmat *BAbt;          // pointer to BAbt in qp_in
+    struct blasfeo_dmat *RSQrq;         // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *z_alg;         // pointer to output z at t = 0
+    bool *set_sim_guess;                 // indicate if initialization for integrator is set from outside
+    struct blasfeo_dvec *sim_guess;     // initializations for integrator
+    // struct blasfeo_dvec *z;             // pointer to (input) z in nlp_out at current stage
+    struct blasfeo_dmat *dzduxt;        // pointer to dzdux transposed
+    void *sim_solver;                   // sim solver memory
+} ocp_nlp_dynamics_cont_memory;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat hess;
+    sim_in *sim_in;
+    sim_out *sim_out;
+    void *sim_solver;  // sim solver workspace
+} ocp_nlp_dynamics_cont_workspace;
+
+acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    void *sim_model;
+    // double *state_transition; // TODO
+    double T;  // simulation time
+} ocp_nlp_dynamics_cont_model;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_dynamics_cont_config_initialize_default(void *config);
+//
+void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
new file mode 100644
index 000000000..8b2a6177b
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h
@@ -0,0 +1,199 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;   // number of states at the current stage
+    int nu;   // number of inputs at the current stage
+    int nx1;  // number of states at the next stage
+    int nu1;  // number of inputes at the next stage
+} ocp_nlp_dynamics_disc_dims;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
+//
+void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_dims_initialize(void *config, void *dims, int nx, int nu, int nx1,
+                                           int nu1, int nz);
+
+//
+void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_dynamics_disc_opts;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts);
+//
+int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_,
+                                        void *mem_, void *work_);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out at current stage
+    struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage
+    struct blasfeo_dvec *ux1;    // pointer to ux in nlp_out at next stage
+    struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage
+    struct blasfeo_dvec *pi;     // pointer to pi in nlp_out at current stage
+    struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage
+    struct blasfeo_dmat *BAbt;   // pointer to BAbt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_dynamics_disc_memory;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_nv;
+} ocp_nlp_dynamics_disc_workspace;
+
+acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    external_function_generic *disc_dyn_fun;
+    external_function_generic *disc_dyn_fun_jac;
+    external_function_generic *disc_dyn_fun_jac_hess;
+} ocp_nlp_dynamics_disc_model;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_dynamics_disc_config_initialize_default(void *config);
+//
+void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
new file mode 100644
index 000000000..cd26788a5
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_reg ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+
+
+
+/* dims */
+
+//typedef ocp_qp_dims ocp_nlp_reg_dims;
+typedef struct
+{
+    int *nx;
+    int *nu;
+    int *nbu;
+    int *nbx;
+    int *ng;
+    int N;
+} ocp_nlp_reg_dims;
+
+//
+acados_size_t ocp_nlp_reg_dims_calculate_size(int N);
+//
+ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory);
+//
+void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value);
+
+
+
+/* config */
+
+typedef struct
+{
+    /* dims */
+    acados_size_t (*dims_calculate_size)(int N);
+    ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory);
+    void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value);
+    /* opts */
+    acados_size_t (*opts_calculate_size)(void);
+    void *(*opts_assign)(void *raw_memory);
+    void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts);
+    void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value);
+    /* memory */
+    acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts);
+    void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+    void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value);
+    void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory);
+    void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    /* functions */
+    void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+    void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+} ocp_nlp_reg_config;
+
+//
+acados_size_t ocp_nlp_reg_config_calculate_size(void);
+//
+void *ocp_nlp_reg_config_assign(void *raw_memory);
+
+
+
+/* regularization help functions */
+void acados_reconstruct_A(int dim, double *A, double *V, double *d);
+void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
+void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
new file mode 100644
index 000000000..df3136168
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h
@@ -0,0 +1,149 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double delta;
+    double epsilon;
+//    double gamma; // 0.0
+} ocp_nlp_reg_convexify_opts;
+
+//
+acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct {
+    double *R;
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+    double *reg_hess; // TODO move to workspace
+
+    struct blasfeo_dmat Q_tilde;
+    struct blasfeo_dmat Q_bar;
+    struct blasfeo_dmat BAQ;
+    struct blasfeo_dmat L;
+    struct blasfeo_dmat delta_eye;
+    struct blasfeo_dmat St_copy;
+
+    struct blasfeo_dmat *original_RSQrq;
+    struct blasfeo_dmat tmp_RSQ;
+
+	struct blasfeo_dvec tmp_nuxM;
+	struct blasfeo_dvec tmp_nbgM;
+
+//    struct blasfeo_dvec grad;
+//    struct blasfeo_dvec b2;
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec **rq;  // pointer to rq in qp_in
+    struct blasfeo_dmat **BAbt;  // pointer to BAbt in qp_in
+    struct blasfeo_dvec **b;  // pointer to b in qp_in
+    struct blasfeo_dmat **DCt;  // pointer to DCt in qp_in
+    struct blasfeo_dvec **ux;  // pointer to ux in qp_out
+    struct blasfeo_dvec **pi;  // pointer to pi in qp_out
+    struct blasfeo_dvec **lam;  // pointer to lam in qp_out
+	int **idxb; // pointer to idxb in qp_in
+
+} ocp_nlp_reg_convexify_memory;
+
+//
+acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
new file mode 100644
index 000000000..f6bd7dcaf
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double epsilon;
+} ocp_nlp_reg_mirror_opts;
+
+//
+acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_mirror_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_mirror_memory;
+
+//
+acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
new file mode 100644
index 000000000..8d2b6ecc1
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h
@@ -0,0 +1,120 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int dummy;
+} ocp_nlp_reg_noreg_opts;
+
+//
+acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    int dummy;
+} ocp_nlp_reg_noreg_memory;
+
+//
+acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+// not needed
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
new file mode 100644
index 000000000..104c29720
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h
@@ -0,0 +1,124 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double epsilon;
+} ocp_nlp_reg_project_opts;
+
+//
+acados_size_t ocp_nlp_reg_project_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_project_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_project_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_project_memory;
+
+//
+acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
new file mode 100644
index 000000000..e0b854bc1
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
@@ -0,0 +1,135 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo/include/blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double thr_eig;
+    double min_eig;
+    double min_pivot;
+	int pivoting;
+} ocp_nlp_reg_project_reduc_hess_opts;
+
+//
+acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat L; // TODO move to workspace
+    struct blasfeo_dmat L2; // TODO move to workspace
+    struct blasfeo_dmat L3; // TODO move to workspace
+    struct blasfeo_dmat Ls; // TODO move to workspace
+    struct blasfeo_dmat P; // TODO move to workspace
+    struct blasfeo_dmat AL; // TODO move to workspace
+
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat **BAbt;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_project_reduc_hess_memory;
+
+//
+acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
new file mode 100644
index 000000000..81a4afd03
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h
@@ -0,0 +1,171 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    double tol_stat;     // exit tolerance on stationarity condition
+    double tol_eq;       // exit tolerance on equality constraints
+    double tol_ineq;     // exit tolerance on inequality constraints
+    double tol_comp;     // exit tolerance on complementarity condition
+    int max_iter;
+    int ext_qp_res;      // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
+    int qp_warm_start;   // qp_warm_start in all but the first sqp iterations
+    bool warm_start_first_qp; // to set qp_warm_start in first iteration
+    int rti_phase;       // only phase 0 at the moment 
+    int print_level;     // verbosity
+    int initialize_t_slacks;  // 0-false or 1-true
+
+} ocp_nlp_sqp_opts;
+
+//
+acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+
+    double time_qp_sol;
+    double time_qp_solver_call;
+    double time_qp_xcond;
+    double time_lin;
+    double time_reg;
+    double time_tot;
+    double time_glob;
+    double time_sim;
+    double time_sim_la;
+    double time_sim_ad;
+
+    // statistics
+    double *stat;
+    int stat_m;
+    int stat_n;
+
+    int status;
+    int sqp_iter;
+
+} ocp_nlp_sqp_memory;
+
+//
+acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_);
+//
+void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+
+    // temp QP in & out (to be used as workspace in param sens)
+    ocp_qp_in *tmp_qp_in;
+    ocp_qp_out *tmp_qp_out;
+
+    // qp residuals
+    ocp_qp_res *qp_res;
+    ocp_qp_res_ws *qp_res_ws;
+
+} ocp_nlp_sqp_workspace;
+
+//
+acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out,
+                void *args, void *mem, void *work_);
+//
+void ocp_nlp_sqp_config_initialize_default(void *config_);
+//
+int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
new file mode 100644
index 000000000..6f16594d2
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h
@@ -0,0 +1,172 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    int compute_dual_sol;
+    int ext_qp_res;           // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
+    int qp_warm_start;        // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp.
+    bool warm_start_first_qp; // to set qp_warm_start in first iteration
+    int rti_phase;            // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both)
+    int print_level;     // verbosity
+
+} ocp_nlp_sqp_rti_opts;
+
+//
+acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_);
+//
+void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory);
+//
+void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_);
+//
+void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_);
+//
+void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage,
+    const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+
+    double time_qp_sol;
+    double time_qp_solver_call;
+    double time_qp_xcond;
+    double time_lin;
+    double time_reg;
+    double time_tot;
+    double time_glob;
+
+    // statistics
+    double *stat;
+    int stat_m;
+    int stat_n;
+
+    int status;
+
+} ocp_nlp_sqp_rti_memory;
+
+//
+acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_);
+//
+void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_,
+    void *raw_memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+
+    // temp QP in & out (to be used as workspace in param sens)
+    ocp_qp_in *tmp_qp_in;
+    ocp_qp_out *tmp_qp_out;
+
+    // qp residuals
+    ocp_qp_res *qp_res;
+    ocp_qp_res_ws *qp_res_ws;
+
+
+} ocp_nlp_sqp_rti_workspace;
+
+//
+acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+void ocp_nlp_sqp_rti_preparation_step(void *config_, void *dims_,
+    void *nlp_in_, void *nlp_out_, void *opts, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_rti_feedback_step(void *config_, void *dims_,
+    void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
+//
+int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_rti_config_initialize_default(void *config_);
+//
+int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_,
+    void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+/// @}
+/// @}
+/// @}
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
new file mode 100644
index 000000000..bba55f8f7
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h
@@ -0,0 +1,182 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_H_
+#define ACADOS_OCP_QP_OCP_QP_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_ocp_qp.h"
+#include "hpipm/include/hpipm_d_ocp_qp_dim.h"
+#include "hpipm/include/hpipm_d_ocp_qp_res.h"
+#include "hpipm/include/hpipm_d_ocp_qp_sol.h"
+// acados
+#include "acados/utils/types.h"
+
+
+
+typedef struct d_ocp_qp_dim ocp_qp_dims;
+typedef struct d_ocp_qp ocp_qp_in;
+typedef struct d_ocp_qp_sol ocp_qp_out;
+typedef struct d_ocp_qp_res ocp_qp_res;
+typedef struct d_ocp_qp_res_ws ocp_qp_res_ws;
+
+
+
+#ifndef QP_SOLVER_CONFIG_
+#define QP_SOLVER_CONFIG_
+typedef struct
+{
+    void (*dims_set)(void *config_, void *dims_, int stage, const char *field, int* value);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+} qp_solver_config;
+#endif
+
+
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config, int N);
+    void *(*dims_assign)(void *config, int N, void *raw_memory);
+    void (*dims_set)(void *config, void *dims_, int stage, const char *field, int* value);
+    void (*dims_get)(void *config, void *dims, const char *field, void* value);
+    // TODO add config everywhere !!!!!
+    acados_size_t (*opts_calculate_size)(void *dims);
+    void *(*opts_assign)(void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *dims, void *opts);
+    void (*opts_update)(void *dims, void *opts);
+    void (*opts_set)(void *opts_, const char *field, void* value);
+    acados_size_t (*memory_calculate_size)(void *dims, void *opts);
+    void *(*memory_assign)(void *dims, void *opts, void *raw_memory);
+    void (*memory_get)(void *config, void *mem, const char *field, void* value);
+    acados_size_t (*workspace_calculate_size)(void *dims, void *opts);
+    int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+} ocp_qp_xcond_config;
+
+
+
+/// Struct containing metrics of the qp solver.
+#ifndef QP_INFO_
+#define QP_INFO_
+typedef struct
+{
+    double solve_QP_time;
+    double condensing_time;
+    double interface_time;
+    double total_time;
+    int num_iter;
+    int t_computed;
+} qp_info;
+#endif
+
+
+
+/* config */
+//
+acados_size_t ocp_qp_solver_config_calculate_size();
+//
+qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory);
+//
+acados_size_t ocp_qp_condensing_config_calculate_size();
+//
+ocp_qp_xcond_config *ocp_qp_condensing_config_assign(void *raw_memory);
+
+
+/* dims */
+//
+acados_size_t ocp_qp_dims_calculate_size(int N);
+//
+ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory);
+//
+void ocp_qp_dims_set(void *config_, void *dims, int stage, const char *field, int* value);
+//
+void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, int* value);
+
+
+/* in */
+//
+acados_size_t ocp_qp_in_calculate_size(ocp_qp_dims *dims);
+//
+ocp_qp_in *ocp_qp_in_assign(ocp_qp_dims *dims, void *raw_memory);
+
+
+/* out */
+//
+acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims);
+//
+ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory);
+
+/* res */
+//
+acados_size_t ocp_qp_res_calculate_size(ocp_qp_dims *dims);
+//
+ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory);
+//
+acados_size_t ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims);
+//
+ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory);
+//
+void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws);
+//
+void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]);
+
+
+/* misc */
+//
+void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out);
+//
+void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out);
+//
+void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_COMMON_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
new file mode 100644
index 000000000..50b80850c
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
+#define ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+
+typedef struct
+{
+    int N;
+    int *nx;
+    int *nu;
+    int *nb;
+    int *nc;
+    double **A;
+    double **B;
+    double **b;
+    double **Q;
+    double **S;
+    double **R;
+    double **q;
+    double **r;
+    int **idxb;
+    double **lb;
+    double **ub;
+    double **Cx;
+    double **Cu;
+    double **lc;
+    double **uc;
+} colmaj_ocp_qp_in;
+
+typedef struct
+{
+    double **x;
+    double **u;
+    double **pi;
+    double **lam;
+} colmaj_ocp_qp_out;
+
+typedef struct
+{
+    double **res_r;
+    double **res_q;
+    double **res_ls;
+    double **res_us;
+    double **res_b;
+    double **res_d_lb;
+    double **res_d_ub;
+    double **res_d_lg;
+    double **res_d_ug;
+    double **res_d_ls;
+    double **res_d_us;
+    double **res_m_lb;
+    double **res_m_ub;
+    double **res_m_lg;
+    double **res_m_ug;
+    double **res_m_ls;
+    double **res_m_us;
+    double res_nrm_inf[4];
+} colmaj_ocp_qp_res;
+
+//
+acados_size_t colmaj_ocp_qp_in_calculate_size(ocp_qp_dims *dims);
+//
+char *assign_colmaj_ocp_qp_in(ocp_qp_dims *dims, colmaj_ocp_qp_in **qp_in, void *ptr);
+//
+acados_size_t colmaj_ocp_qp_out_calculate_size(ocp_qp_dims *dims);
+//
+char *assign_colmaj_ocp_qp_out(ocp_qp_dims *dims, colmaj_ocp_qp_out **qp_out, void *ptr);
+//
+acados_size_t colmaj_ocp_qp_res_calculate_size(ocp_qp_dims *dims);
+//
+char *assign_colmaj_ocp_qp_res(ocp_qp_dims *dims, colmaj_ocp_qp_res **qp_res, void *ptr);
+//
+void convert_colmaj_to_ocp_qp_in(colmaj_ocp_qp_in *cm_qp_in, ocp_qp_in *qp_in);
+//
+void convert_ocp_qp_out_to_colmaj(ocp_qp_out *qp_out, colmaj_ocp_qp_out *cm_qp_out);
+//
+void convert_ocp_qp_res_to_colmaj(ocp_qp_res *qp_res, colmaj_ocp_qp_res *cm_qp_res);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
new file mode 100644
index 000000000..14ac97bbf
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h
@@ -0,0 +1,117 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
+#define ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_ocp_qp_red.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+
+
+typedef struct
+{
+    ocp_qp_dims *orig_dims;
+    ocp_qp_dims *red_dims; // dims of reduced qp
+    dense_qp_dims *fcond_dims;
+} ocp_qp_full_condensing_dims;
+
+
+
+typedef struct ocp_qp_full_condensing_opts_
+{
+    struct d_cond_qp_arg *hpipm_cond_opts;
+    struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
+//    dense_qp_dims *fcond_dims;  // TODO(all): move to dims
+    int cond_hess; // 0 cond only rhs, 1 cond hess + rhs
+    int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
+    int ric_alg;
+    int mem_qp_in; // allocate qp_in in memory
+} ocp_qp_full_condensing_opts;
+
+
+
+typedef struct ocp_qp_full_condensing_memory_
+{
+    struct d_cond_qp_ws *hpipm_cond_work;
+    struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
+    // in memory
+    dense_qp_in *fcond_qp_in;
+    dense_qp_out *fcond_qp_out;
+    ocp_qp_in *red_qp; // reduced qp
+    ocp_qp_out *red_sol; // reduced qp sol
+    // only pointer
+    ocp_qp_in *ptr_qp_in;
+    qp_info *qp_out_info; // info in fcond_qp_in
+    double time_qp_xcond;
+} ocp_qp_full_condensing_memory;
+
+
+
+//
+acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims);
+//
+void *ocp_qp_full_condensing_opts_assign(void *dims, void *raw_memory);
+//
+void ocp_qp_full_condensing_opts_initialize_default(void *dims, void *opts_);
+//
+void ocp_qp_full_condensing_opts_update(void *dims, void *opts_);
+//
+void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value);
+//
+acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims, void *opts_);
+//
+void *ocp_qp_full_condensing_memory_assign(void *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void *opts_);
+//
+int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work);
+//
+int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work);
+//
+void ocp_qp_full_condensing_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
new file mode 100644
index 000000000..84e874863
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_HPIPM_H_
+#define ACADOS_OCP_QP_OCP_QP_HPIPM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_ocp_qp_ipm.h"
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+
+
+// struct of arguments to the solver
+// TODO(roversch): why not make this a typedef of the underlying struct?
+typedef struct ocp_qp_hpipm_opts_
+{
+    struct d_ocp_qp_ipm_arg *hpipm_opts;
+} ocp_qp_hpipm_opts;
+
+
+
+// TODO(roversch): why not make this a typedef of the underlying struct?
+// struct of the solver memory
+typedef struct ocp_qp_hpipm_memory_
+{
+    struct d_ocp_qp_ipm_ws *hpipm_workspace;
+    double time_qp_solver_call;
+    int iter;
+
+} ocp_qp_hpipm_memory;
+
+
+
+//
+acados_size_t ocp_qp_hpipm_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void ocp_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
+//
+void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value);
+//
+acados_size_t ocp_qp_hpipm_memory_calculate_size(void *config, void *dims, void *opts_);
+//
+void *ocp_qp_hpipm_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_hpipm_workspace_calculate_size(void *config, void *dims, void *opts_);
+//
+int ocp_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_hpipm_eval_sens(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_hpipm_config_initialize_default(void *config);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_HPIPM_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
new file mode 100644
index 000000000..37d1c0a33
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h
@@ -0,0 +1,129 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_HPMPC_H_
+#define ACADOS_OCP_QP_OCP_QP_HPMPC_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef enum hpmpc_options_t_ { HPMPC_DEFAULT_ARGUMENTS } hpmpc_options_t;
+
+typedef struct ocp_qp_hpmpc_opts_
+{
+    double tol;
+    int max_iter;
+    double mu0;
+    double alpha_min;
+    int warm_start;
+    int N2;  // horizion length of the partially condensed problem
+
+    // partial tightening
+    double sigma_mu;
+    int N;
+    int M;
+} ocp_qp_hpmpc_opts;
+
+// struct of the solver memory
+typedef struct ocp_qp_hpmpc_memory_
+{
+    struct blasfeo_dvec *hpi;
+    double *stats;
+
+    // workspace
+    void *hpmpc_work;  // raw workspace
+
+    // partial tightening-specific (init of extra variables)
+    struct blasfeo_dvec *lam0;
+    struct blasfeo_dvec *ux0;
+    struct blasfeo_dvec *pi0;
+    struct blasfeo_dvec *t0;
+
+    // 2. workspace
+    struct blasfeo_dmat *hsL;
+    struct blasfeo_dmat *hsric_work_mat;
+    struct blasfeo_dmat sLxM;
+    struct blasfeo_dmat sPpM;
+
+    struct blasfeo_dvec *hsQx;
+    struct blasfeo_dvec *hsqx;
+    struct blasfeo_dvec *hstinv;
+    struct blasfeo_dvec *hsrq;
+    struct blasfeo_dvec *hsdux;
+
+    struct blasfeo_dvec *hsdlam;
+    struct blasfeo_dvec *hsdt;
+    struct blasfeo_dvec *hsdpi;
+    struct blasfeo_dvec *hslamt;
+
+    struct blasfeo_dvec *hsPb;
+
+    void *work_ric;
+
+    int out_iter;
+
+    double time_qp_solver_call;
+    int iter;
+
+} ocp_qp_hpmpc_memory;
+
+acados_size_t ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims);
+//
+void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
+//
+void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+acados_size_t ocp_qp_hpmpc_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void *ocp_qp_hpmpc_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_hpmpc_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_hpmpc_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_HPMPC_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
new file mode 100644
index 000000000..e1b789b96
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h
@@ -0,0 +1,146 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_OOQP_H_
+#define ACADOS_OCP_QP_OCP_QP_OOQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+enum ocp_qp_ooqp_termination_code
+{
+  SPARSE_SUCCESSFUL_TERMINATION = 0,
+  SPARSE_NOT_FINISHED,
+  SPARSE_MAX_ITS_EXCEEDED,
+  SPARSE_INFEASIBLE,
+  SPARSE_UNKNOWN
+};
+
+typedef struct ocp_qp_ooqp_opts_
+{
+    int printLevel;
+    int useDiagonalWeights;  // TODO(dimitris): implement option
+    int fixHessian;
+    int fixHessianSparsity;
+    int fixDynamics;
+    int fixDynamicsSparsity;
+    int fixInequalities;
+    int fixInequalitiesSparsity;
+} ocp_qp_ooqp_opts;
+
+typedef struct ocp_qp_ooqp_workspace_
+{
+    double *x;
+    double *gamma;
+    double *phi;
+    double *y;
+    double *z;
+    double *lambda;
+    double *pi;
+    double objectiveValue;
+    int *tmpInt;    // temporary vector to sort indicies sparse matrices
+    double *tmpReal;  // temporary vector to sort data of sparse matrices
+    // int ierr;
+} ocp_qp_ooqp_workspace;
+
+typedef struct ocp_qp_ooqp_memory_
+{
+    int firstRun;
+    double *c;
+    int nx;
+    int *irowQ;
+    int nnzQ;
+    int *jcolQ;
+    int *orderQ;
+    double *dQ;
+    double *xlow;
+    char *ixlow;
+    double *xupp;
+    char *ixupp;
+    int *irowA;
+    int nnzA;
+    int *jcolA;
+    int *orderA;
+    double *dA;
+    double *bA;
+    int my;
+    int *irowC;
+    int nnzC;
+    int *jcolC;
+    int *orderC;
+    double *dC;
+    double *clow;
+    int mz;
+    char *iclow;
+    double *cupp;
+    char *icupp;
+    int nnz;  // max(nnzQ, nnzA, nnzC)
+    double time_qp_solver_call;
+    int iter;
+
+} ocp_qp_ooqp_memory;
+
+//
+acados_size_t ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims);
+//
+void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
+//
+void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+acados_size_t ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
+                void *work_);
+//
+void ocp_qp_ooqp_destroy(void *mem_, void *work);
+//
+void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_ooqp_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_OOQP_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
new file mode 100644
index 000000000..55321a46b
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_OSQP_H_
+#define ACADOS_OCP_QP_OCP_QP_OSQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// osqp
+#include "osqp/include/types.h"
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef struct ocp_qp_osqp_opts_
+{
+    OSQPSettings *osqp_opts;
+} ocp_qp_osqp_opts;
+
+
+typedef struct ocp_qp_osqp_memory_
+{
+    c_int first_run;
+
+    c_float *q;
+    c_float *l;
+    c_float *u;
+
+    c_int P_nnzmax;
+    c_int *P_i;
+    c_int *P_p;
+    c_float *P_x;
+
+    c_int A_nnzmax;
+    c_int *A_i;
+    c_int *A_p;
+    c_float *A_x;
+
+    OSQPData *osqp_data;
+    OSQPWorkspace *osqp_work;
+
+    double time_qp_solver_call;
+    int iter;
+
+} ocp_qp_osqp_memory;
+
+acados_size_t ocp_qp_osqp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_qp_osqp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_qp_osqp_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void ocp_qp_osqp_opts_update(void *config, void *dims, void *opts_);
+//
+acados_size_t ocp_qp_osqp_memory_calculate_size(void *config, void *dims, void *opts_);
+//
+void *ocp_qp_osqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, void *opts_);
+//
+int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_osqp_config_initialize_default(void *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_OSQP_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
new file mode 100644
index 000000000..b95a11114
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h
@@ -0,0 +1,123 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
+#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_ocp_qp_red.h"
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+
+
+
+typedef struct
+{
+    ocp_qp_dims *orig_dims;
+    ocp_qp_dims *red_dims; // dims of reduced qp
+    ocp_qp_dims *pcond_dims;
+    int *block_size;
+    int N2;
+    int N2_bkp;
+} ocp_qp_partial_condensing_dims;
+
+
+
+typedef struct ocp_qp_partial_condensing_opts_
+{
+    struct d_part_cond_qp_arg *hpipm_pcond_opts;
+    struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts;
+//    ocp_qp_dims *pcond_dims;  // TODO(all): move to dims
+//    int *block_size;
+    int N2;
+    int N2_bkp;
+//    int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol
+    int ric_alg;
+    int mem_qp_in; // allocate qp_in in memory
+} ocp_qp_partial_condensing_opts;
+
+
+
+typedef struct ocp_qp_partial_condensing_memory_
+{
+    struct d_part_cond_qp_ws *hpipm_pcond_work;
+    struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work;
+    // in memory
+    ocp_qp_in *pcond_qp_in;
+    ocp_qp_out *pcond_qp_out;
+    ocp_qp_in *red_qp; // reduced qp
+    ocp_qp_out *red_sol; // reduced qp sol
+    // only pointer
+    ocp_qp_in *ptr_qp_in;
+    ocp_qp_in *ptr_pcond_qp_in;
+    qp_info *qp_out_info; // info in pcond_qp_in
+    double time_qp_xcond;
+} ocp_qp_partial_condensing_memory;
+
+
+
+//
+acados_size_t ocp_qp_partial_condensing_opts_calculate_size(void *dims);
+//
+void *ocp_qp_partial_condensing_opts_assign(void *dims, void *raw_memory);
+//
+void ocp_qp_partial_condensing_opts_initialize_default(void *dims, void *opts_);
+//
+void ocp_qp_partial_condensing_opts_update(void *dims, void *opts_);
+//
+void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value);
+//
+acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims, void *opts_);
+//
+void *ocp_qp_partial_condensing_memory_assign(void *dims, void *opts, void *raw_memory);
+//
+acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, void *opts_);
+//
+int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work);
+//
+int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work);
+//
+void ocp_qp_partial_condensing_config_initialize_default(void *config_);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
new file mode 100644
index 000000000..348627c67
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h
@@ -0,0 +1,121 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
+#define ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "qpDUNES.h"
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef enum qpdunes_options_t_ {
+    QPDUNES_DEFAULT_ARGUMENTS,
+    QPDUNES_LINEAR_MPC,     // TODO(dimitris): partly implemented
+    QPDUNES_NONLINEAR_MPC,  // TODO(dimitris): not implemented yet
+    QPDUNES_ACADO_SETTINGS
+} qpdunes_options_t;
+
+typedef enum { QPDUNES_WITH_QPOASES, QPDUNES_WITH_CLIPPING } qpdunes_stage_qp_solver_t;
+
+typedef struct ocp_qp_qpdunes_opts_
+{
+    qpOptions_t options;
+    qpdunes_stage_qp_solver_t stageQpSolver;
+    int warmstart;  // warmstart = 0: all multipliers set to zero, warmstart = 1: use previous mult.
+    bool isLinearMPC;
+} ocp_qp_qpdunes_opts;
+
+typedef struct ocp_qp_qpdunes_memory_
+{
+    int firstRun;
+    int nx;
+    int nu;
+    int nz;
+    int nDmax;  // max(dims->ng)
+    qpData_t qpData;
+    double time_qp_solver_call;
+    int iter;
+
+} ocp_qp_qpdunes_memory;
+
+typedef struct ocp_qp_qpdunes_workspace_
+{
+    double *H;
+    double *Q;
+    double *R;
+    double *S;
+    double *g;
+    double *ABt;
+    double *b;
+    double *Ct;
+    double *lc;
+    double *uc;
+    double *zLow;
+    double *zUpp;
+} ocp_qp_qpdunes_workspace;
+
+//
+acados_size_t ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims);
+//
+void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory);
+//
+void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+acados_size_t ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_);
+//
+int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_,
+                   void *work_);
+//
+void ocp_qp_qpdunes_free_memory(void *mem_);
+//
+void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void ocp_qp_qpdunes_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_QPDUNES_H_
diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
new file mode 100644
index 000000000..9d4f309bd
--- /dev/null
+++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h
@@ -0,0 +1,151 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
+#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/types.h"
+
+
+
+typedef struct
+{
+    ocp_qp_dims *orig_dims;
+    void *xcond_dims;
+} ocp_qp_xcond_solver_dims;
+
+
+
+typedef struct ocp_qp_xcond_solver_opts_
+{
+    void *xcond_opts;
+    void *qp_solver_opts;
+} ocp_qp_xcond_solver_opts;
+
+
+
+typedef struct ocp_qp_xcond_solver_memory_
+{
+    void *xcond_memory;
+    void *solver_memory;
+    void *xcond_qp_in;
+    void *xcond_qp_out;
+} ocp_qp_xcond_solver_memory;
+
+
+
+typedef struct ocp_qp_xcond_solver_workspace_
+{
+    void *xcond_work;
+    void *qp_solver_work;
+} ocp_qp_xcond_solver_workspace;
+
+
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config, int N);
+    ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory);
+    void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
+    acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims);
+    void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
+    void (*opts_update)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    acados_size_t (*memory_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
+    void *(*memory_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts, void *raw_memory);
+    void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
+    acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts);
+    int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work);
+    void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work);
+    qp_solver_config *qp_solver;  // either ocp_qp_solver or dense_solver
+    ocp_qp_xcond_config *xcond;
+} ocp_qp_xcond_solver_config;  // pcond - partial condensing or fcond - full condensing
+
+
+
+/* config */
+//
+acados_size_t ocp_qp_xcond_solver_config_calculate_size();
+//
+ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory);
+
+/* dims */
+//
+acados_size_t ocp_qp_xcond_solver_dims_calculate_size(void *config, int N);
+//
+ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config, int N, void *raw_memory);
+//
+void ocp_qp_xcond_solver_dims_set_(void *config, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value);
+
+/* opts */
+//
+acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims);
+//
+void *ocp_qp_xcond_solver_opts_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory);
+//
+void ocp_qp_xcond_solver_opts_initialize_default(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
+//
+void ocp_qp_xcond_solver_opts_update(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
+//
+void ocp_qp_xcond_solver_opts_set_(void *config_, void *opts_, const char *field, void* value);
+
+/* memory */
+//
+acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
+//
+void *ocp_qp_xcond_solver_memory_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_, void *raw_memory);
+
+/* workspace */
+//
+acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
+
+/* config */
+//
+int ocp_qp_xcond_solver(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *mem_, void *work_);
+
+//
+void ocp_qp_xcond_solver_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_
diff --git a/third_party/acados/include/acados/sim/sim_collocation_utils.h b/third_party/acados/include/acados/sim/sim_collocation_utils.h
new file mode 100644
index 000000000..850dab50d
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_collocation_utils.h
@@ -0,0 +1,91 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
+#define ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/utils/types.h"
+
+
+
+enum Newton_type_collocation
+{
+    exact = 0,
+    simplified_in,
+    simplified_inis
+};
+
+
+
+typedef struct
+{
+    enum Newton_type_collocation type;
+    double *eig;
+    double *low_tria;
+    bool single;
+    bool freeze;
+
+    double *transf1;
+    double *transf2;
+
+    double *transf1_T;
+    double *transf2_T;
+} Newton_scheme;
+
+
+
+//
+acados_size_t gauss_nodes_work_calculate_size(int ns);
+//
+void gauss_nodes(int ns, double *nodes, void *raw_memory);
+//
+acados_size_t gauss_simplified_work_calculate_size(int ns);
+//
+void gauss_simplified(int ns, Newton_scheme *scheme, void *work);
+//
+acados_size_t butcher_table_work_calculate_size(int ns);
+//
+void butcher_table(int ns, double *nodes, double *b, double *A, void *work);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_
diff --git a/third_party/acados/include/acados/sim/sim_common.h b/third_party/acados/include/acados/sim/sim_common.h
new file mode 100644
index 000000000..177f0893d
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_common.h
@@ -0,0 +1,221 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_COMMON_H_
+#define ACADOS_SIM_SIM_COMMON_H_
+
+#include <stdbool.h>
+
+#include "acados/sim/sim_collocation_utils.h"
+#include "acados/utils/timing.h"
+#include "acados/utils/types.h"
+
+#include "acados/utils/external_function_generic.h"
+
+// maximum number of integration stages
+#define NS_MAX 15
+
+
+
+typedef enum
+{
+    // ERK and LIFTED_ERK
+    EXPL_ODE_FUN,
+    EXPL_ODE_HES,  // wrt x and u ???
+    EXPL_VDE_FOR,
+    EXPL_VDE_ADJ,
+    // IRK
+    IMPL_ODE_FUN,
+    IMPL_ODE_FUN_JAC_X_XDOT,
+    IMPL_ODE_JAC_X_XDOT_U,
+    IMPL_ODE_FUN_JAC_X_XDOT_U,
+    IMPL_ODE_HESS,
+    // gnsf
+    PHI_FUN,
+    PHI_FUN_JAC_Y,
+    PHI_JAC_Y_UHAT,
+    LO_FUN,
+    GET_GNSF_MATRICES
+} sim_function_t;
+
+
+
+typedef struct
+{
+    void *dims;
+
+    double *x;  // x[NX] - initial state value for simulation
+    double *u;  // u[NU] - control - constant over simulation time
+
+    double *S_forw;  // forward seed [Sx, Su]
+    double *S_adj;   // backward seed
+
+    bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)]
+
+    void *model;
+
+    double T;  // simulation time
+
+} sim_in;
+
+
+
+typedef struct
+{
+    double CPUtime;  // in seconds
+    double LAtime;   // in seconds
+    double ADtime;   // in seconds
+
+} sim_info;
+
+
+
+typedef struct
+{
+    double *xn;      // xn[NX]
+    double *S_forw;  // S_forw[NX*(NX+NU)]
+    double *S_adj;   //
+    double *S_hess;  //
+
+    double *zn;           // z - algebraic variables - reported at start of simulation interval
+    double *S_algebraic;  // sensitivities of reported value of algebraic variables w.r.t.
+                          // initial stat & control (x_n,u)
+
+    double *grad;  // gradient correction
+
+    sim_info *info;
+
+} sim_out;
+
+
+
+typedef struct
+{
+    int ns;  // number of integration stages
+
+    int num_steps;
+    int num_forw_sens;
+
+    int tableau_size;  // check that is consistent with ns
+            // only update when butcher tableau is changed
+            // kind of private -> no setter!
+    double *A_mat;
+    double *c_vec;
+    double *b_vec;
+
+    bool sens_forw;
+    bool sens_adj;
+    bool sens_hess;
+
+    bool output_z;        // 1 -- if zn should be computed
+    bool sens_algebraic;  // 1 -- if S_algebraic should be computed
+    bool exact_z_output;  // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations
+
+    // for explicit integrators: newton_iter == 0 && scheme == NULL
+    // && jac_reuse=false
+    int newton_iter;
+    bool jac_reuse;
+    Newton_scheme *scheme;
+
+    // workspace
+    void *work;
+
+} sim_opts;
+
+
+
+typedef struct
+{
+    int (*evaluate)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
+    int (*precompute)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work);
+    // opts
+    acados_size_t (*opts_calculate_size)(void *config_, void *dims);
+    void *(*opts_assign)(void *config_, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config_, void *dims, void *opts);
+    void (*opts_update)(void *config_, void *dims, void *opts);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
+    void (*opts_get)(void *config_, void *opts_, const char *field, void *value);
+    // mem
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    int (*memory_set)(void *config, void *dims, void *mem, const char *field, void *value);
+    int (*memory_set_to_zero)(void *config, void *dims, void *opts, void *mem, const char *field);
+    void (*memory_get)(void *config, void *dims, void *mem, const char *field, void *value);
+    // work
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    // model
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    int (*model_set)(void *model, const char *field, void *value);
+    // config
+    void (*config_initialize_default)(void *config);
+    // dims
+    acados_size_t (*dims_calculate_size)();
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_set)(void *config, void *dims, const char *field, const int *value);
+    void (*dims_get)(void *config, void *dims, const char *field, int *value);
+
+} sim_config;
+
+
+
+/* config */
+//
+acados_size_t sim_config_calculate_size();
+//
+sim_config *sim_config_assign(void *raw_memory);
+
+/* in */
+//
+acados_size_t sim_in_calculate_size(void *config, void *dims);
+//
+sim_in *sim_in_assign(void *config, void *dims, void *raw_memory);
+//
+int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value);
+
+/* out */
+//
+acados_size_t sim_out_calculate_size(void *config, void *dims);
+//
+sim_out *sim_out_assign(void *config, void *dims, void *raw_memory);
+//
+int sim_out_get_(void *config, void *dims, sim_out *out, const char *field, void *value);
+
+/* opts */
+//
+void sim_opts_set_(sim_opts *opts, const char *field, void *value);
+//
+void sim_opts_get_(sim_config *config, sim_opts *opts, const char *field, void *value);
+
+#endif  // ACADOS_SIM_SIM_COMMON_H_
diff --git a/third_party/acados/include/acados/sim/sim_erk_integrator.h b/third_party/acados/include/acados/sim/sim_erk_integrator.h
new file mode 100644
index 000000000..24a00c707
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_erk_integrator.h
@@ -0,0 +1,143 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
+#define ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/types.h"
+
+
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+} sim_erk_dims;
+
+
+
+typedef struct
+{
+    /* external functions */
+    // explicit ode
+    external_function_generic *expl_ode_fun;
+    // hessian explicit ode
+    external_function_generic *expl_ode_hes;
+    // forward explicit vde
+    external_function_generic *expl_vde_for;
+    // adjoint explicit vde
+    external_function_generic *expl_vde_adj;
+
+} erk_model;
+
+
+
+typedef struct
+{
+	// memory
+	double time_sim;
+	double time_ad;
+	double time_la;
+
+	// workspace structs
+} sim_erk_memory;
+
+
+
+typedef struct
+{
+	// workspace mem
+    double *rhs_forw_in;  // x + S + p
+
+    double *K_traj;         // (stages*nX) or (steps*stages*nX) for adj
+    double *out_forw_traj;  // S or (steps+1)*nX for adj
+
+    double *rhs_adj_in;
+    double *out_adj_tmp;
+    double *adj_traj;
+
+} sim_erk_workspace;
+
+
+
+// dims
+acados_size_t sim_erk_dims_calculate_size();
+void *sim_erk_dims_assign(void *config_, void *raw_memory);
+void sim_erk_dims_set(void *config_, void *dims_, const char *field, const int* value);
+void sim_erk_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+// model
+acados_size_t sim_erk_model_calculate_size(void *config, void *dims);
+void *sim_erk_model_assign(void *config, void *dims, void *raw_memory);
+int sim_erk_model_set(void *model, const char *field, void *value);
+
+// opts
+acados_size_t sim_erk_opts_calculate_size(void *config, void *dims);
+//
+void sim_erk_opts_update(void *config_, void *dims, void *opts_);
+//
+void *sim_erk_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void sim_erk_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void sim_erk_opts_set(void *config_, void *opts_, const char *field, void *value);
+
+
+// memory
+acados_size_t sim_erk_memory_calculate_size(void *config, void *dims, void *opts_);
+//
+void *sim_erk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+//
+int sim_erk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
+
+
+// workspace
+acados_size_t sim_erk_workspace_calculate_size(void *config, void *dims, void *opts_);
+
+//
+int sim_erk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
+//
+void sim_erk_config_initialize_default(void *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_ERK_INTEGRATOR_H_
diff --git a/third_party/acados/include/acados/sim/sim_gnsf.h b/third_party/acados/include/acados/sim/sim_gnsf.h
new file mode 100644
index 000000000..5524b384e
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_gnsf.h
@@ -0,0 +1,367 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_GNSF_H_
+#define ACADOS_SIM_SIM_GNSF_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+
+#include "acados/utils/timing.h"
+#include "acados/utils/types.h"
+#include "acados/sim/sim_common.h"
+
+#include "blasfeo/include/blasfeo_common.h"
+#include "blasfeo/include/blasfeo_d_aux.h"
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+#include "blasfeo/include/blasfeo_d_blas.h"
+#include "blasfeo/include/blasfeo_d_kernel.h"
+#include "blasfeo/include/blasfeo_i_aux_ext_dep.h"
+#include "blasfeo/include/blasfeo_target.h"
+
+/* 
+GNSF - Generalized Nonlinear Static Feedback Model
+has the following form
+https://github.com/acados/acados/files/3359595/gnsf_structure_blo.pdf
+
+Details on the algorithm can be found in master thesis of Jonathan Frey,
+which presents a slightly different format without the terms B_LO, c_LO.
+https://github.com/acados/acados/files/2318322/gnsf_structure.pdf
+https://cdn.syscop.de/publications/Frey2018.pdf
+https://cdn.syscop.de/publications/Frey2019.pdf
+*/
+
+typedef struct
+{
+    int nx; // total number of differential states
+    int nu; // total number of inputs
+    int nz; // total number of algebraic states
+    int nx1; // number of differential states in NSF part
+    int nz1; // number of algebraic states in NSF part
+    int n_out; // output dimension of phi
+    int ny; // dimension of first input of phi
+    int nuhat; // dimension of second input of phi
+
+} sim_gnsf_dims;
+
+
+
+typedef struct
+{
+    /* external functions */
+    // phi: nonlinearity function
+    external_function_generic *phi_fun;
+    external_function_generic *phi_fun_jac_y;
+    external_function_generic *phi_jac_y_uhat;
+
+    // f_lo: linear output function
+    external_function_generic *f_lo_fun_jac_x1_x1dot_u_z;
+
+    // to import model matrices
+    external_function_generic *get_gnsf_matrices;
+
+    // flag indicating, if model defining matrices are imported via external (casadi) function,
+    //    [default]: true -> auto;
+    bool auto_import_gnsf;
+
+    // booleans from structure detection
+    bool nontrivial_f_LO; // indicates if f_LO is constant zero function
+    bool fully_linear; // indicates if model is fully linear LOS
+
+    /* model defining matrices */
+    // TODO: add setters to set manually
+    double *A;
+    double *B;
+    double *C;
+    double *E;
+
+    double *L_x;
+    double *L_xdot;
+    double *L_z;
+    double *L_u;
+
+    double *A_LO;
+    double *B_LO;
+    double *E_LO;
+
+    /* constant vector */
+    double *c;
+    double *c_LO;
+
+    // permutation vector - to have GNSF order of x, z within sim_gnsf only
+    int *ipiv_x;
+    int *ipiv_z;
+
+    double *ipiv_x_double;
+    double *ipiv_z_double;
+
+} gnsf_model;
+
+
+
+// pre_workspace - workspace used in the precomputation phase
+typedef struct
+{
+    struct blasfeo_dmat E11;
+    struct blasfeo_dmat E12;
+    struct blasfeo_dmat E21;
+    struct blasfeo_dmat E22;
+
+    struct blasfeo_dmat A1;
+    struct blasfeo_dmat A2;
+    struct blasfeo_dmat B1;
+    struct blasfeo_dmat B2;
+    struct blasfeo_dmat C1;
+    struct blasfeo_dmat C2;
+
+    struct blasfeo_dmat AA1;
+    struct blasfeo_dmat AA2;
+    struct blasfeo_dmat BB1;
+    struct blasfeo_dmat BB2;
+    struct blasfeo_dmat CC1;
+    struct blasfeo_dmat CC2;
+    struct blasfeo_dmat DD1;
+    struct blasfeo_dmat DD2;
+    struct blasfeo_dmat EE1;
+    struct blasfeo_dmat EE2;
+
+    struct blasfeo_dmat QQ1;
+
+    struct blasfeo_dmat LLZ;
+    struct blasfeo_dmat LLx;
+    struct blasfeo_dmat LLK;
+
+    int *ipivEE1;  // index of pivot vector
+    int *ipivEE2;
+    int *ipivQQ1;
+
+    // for algebraic sensitivity propagation
+    struct blasfeo_dmat Q1;
+
+    // for constant term in NSF
+    struct blasfeo_dvec cc1;
+    struct blasfeo_dvec cc2;
+
+} gnsf_pre_workspace;
+
+
+
+// workspace
+typedef struct
+{
+    double *Z_work;  // used to perform computations to get out->zn
+
+    int *ipiv;  // index of pivot vector
+
+    struct blasfeo_dvec *vv_traj;
+    struct blasfeo_dvec *yy_traj;
+    struct blasfeo_dmat *f_LO_jac_traj;
+
+    struct blasfeo_dvec K2_val;
+    struct blasfeo_dvec x0_traj;
+    struct blasfeo_dvec res_val;
+    struct blasfeo_dvec u0;
+    struct blasfeo_dvec lambda;
+    struct blasfeo_dvec lambda_old;
+
+    struct blasfeo_dvec yyu;
+    struct blasfeo_dvec yyss;
+
+    struct blasfeo_dvec K1_val;
+    struct blasfeo_dvec f_LO_val;
+    struct blasfeo_dvec x1_stage_val;
+    struct blasfeo_dvec Z1_val;
+
+    struct blasfeo_dvec K1u;
+    struct blasfeo_dvec Zu;
+    struct blasfeo_dvec ALOtimesx02;
+    struct blasfeo_dvec BLOtimesu0;
+
+    struct blasfeo_dvec uhat;
+
+    struct blasfeo_dmat J_r_vv;
+    struct blasfeo_dmat J_r_x1u;
+
+    struct blasfeo_dmat dK1_dx1;
+    struct blasfeo_dmat dK1_du;
+    struct blasfeo_dmat dZ_dx1;
+    struct blasfeo_dmat dZ_du;
+    struct blasfeo_dmat J_G2_K1;
+
+    struct blasfeo_dmat dK2_dx1;
+    struct blasfeo_dmat dK2_dvv;
+    struct blasfeo_dmat dxf_dwn;
+    struct blasfeo_dmat S_forw_new;
+    struct blasfeo_dmat S_algebraic_aux;
+
+    struct blasfeo_dmat dPsi_dvv;
+    struct blasfeo_dmat dPsi_dx;
+    struct blasfeo_dmat dPsi_du;
+
+    struct blasfeo_dmat dPHI_dyuhat;
+    struct blasfeo_dvec z0;
+
+    // memory only available if (opts->sens_algebraic)
+    // struct blasfeo_dvec y_one_stage;
+    // struct blasfeo_dvec x0dot_1;
+    // struct blasfeo_dmat dz10_dx1u;  // (nz1) * (nx1+nu);
+    // struct blasfeo_dmat dr0_dvv0;  // (n_out * n_out)
+    // struct blasfeo_dmat f_LO_jac0; // (nx2+nz2) * (2*nx1 + nz1 + nu)
+    // struct blasfeo_dmat sens_z2_rhs; // (nx2 + nz2) * (nx1 + nu)
+    // int *ipiv_vv0;
+
+} gnsf_workspace;
+
+
+
+// memory
+typedef struct
+{
+    bool first_call;
+
+    // simulation time for one step
+    double dt;
+
+    // (scaled) butcher table
+    double *A_dt;
+    double *b_dt;
+    double *c_butcher;
+
+    // value used to initialize integration variables - corresponding to value of phi
+    double *phi_guess;  //  n_out
+
+    struct blasfeo_dmat S_forw;
+    struct blasfeo_dmat S_algebraic;
+
+    // precomputed matrices
+    struct blasfeo_dmat KKv;
+    struct blasfeo_dmat KKx;
+    struct blasfeo_dmat KKu;
+
+    struct blasfeo_dmat YYv;
+    struct blasfeo_dmat YYx;
+    struct blasfeo_dmat YYu;
+
+    struct blasfeo_dmat ZZv;
+    struct blasfeo_dmat ZZx;
+    struct blasfeo_dmat ZZu;
+
+    struct blasfeo_dmat ALO;
+    struct blasfeo_dmat BLO;
+    struct blasfeo_dmat M2_LU;
+    int *ipivM2;
+
+    struct blasfeo_dmat dK2_dx2;
+    struct blasfeo_dmat dK2_du;
+    struct blasfeo_dmat dx2f_dx2u;
+
+    struct blasfeo_dmat Lu;
+
+    // precomputed vectors for constant term in NSF
+    struct blasfeo_dvec KK0;
+    struct blasfeo_dvec YY0;
+    struct blasfeo_dvec ZZ0;
+
+    // for algebraic sensitivities only;
+    // struct blasfeo_dmat *Z0x;
+    // struct blasfeo_dmat *Z0u;
+    // struct blasfeo_dmat *Z0v;
+
+    // struct blasfeo_dmat *Y0x;
+    // struct blasfeo_dmat *Y0u;
+    // struct blasfeo_dmat *Y0v;
+
+    // struct blasfeo_dmat *K0x;
+    // struct blasfeo_dmat *K0u;
+    // struct blasfeo_dmat *K0v;
+
+    // struct blasfeo_dmat *ELO_LU;
+    // int *ipiv_ELO;
+    // struct blasfeo_dmat *ELO_inv_ALO;
+
+    // struct blasfeo_dmat *Lx;
+    // struct blasfeo_dmat *Lxdot;
+    // struct blasfeo_dmat *Lz;
+
+	double time_sim;
+	double time_ad;
+	double time_la;
+
+} sim_gnsf_memory;
+
+
+
+// gnsf dims
+acados_size_t sim_gnsf_dims_calculate_size();
+void *sim_gnsf_dims_assign(void *config_, void *raw_memory);
+
+// get & set functions
+void sim_gnsf_dims_set(void *config_, void *dims_, const char *field, const int *value);
+void sim_gnsf_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+// opts
+acados_size_t sim_gnsf_opts_calculate_size(void *config, void *dims);
+void *sim_gnsf_opts_assign(void *config, void *dims, void *raw_memory);
+void sim_gnsf_opts_initialize_default(void *config, void *dims, void *opts_);
+void sim_gnsf_opts_update(void *config_, void *dims, void *opts_);
+void sim_gnsf_opts_set(void *config_, void *opts_, const char *field, void *value);
+
+// model
+acados_size_t sim_gnsf_model_calculate_size(void *config, void *dims_);
+void *sim_gnsf_model_assign(void *config, void *dims_, void *raw_memory);
+int sim_gnsf_model_set(void *model_, const char *field, void *value);
+
+// precomputation
+int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_,
+                       void *work_);
+
+// workspace & memory
+acados_size_t sim_gnsf_workspace_calculate_size(void *config, void *dims_, void *args);
+acados_size_t sim_gnsf_memory_calculate_size(void *config, void *dims_, void *opts_);
+void *sim_gnsf_memory_assign(void *config, void *dims_, void *opts_, void *raw_memory);
+
+// interface
+void sim_gnsf_config_initialize_default(void *config_);
+
+// integrator
+int sim_gnsf(void *config, sim_in *in, sim_out *out, void *opts, void *mem_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_GNSF_H_
diff --git a/third_party/acados/include/acados/sim/sim_irk_integrator.h b/third_party/acados/include/acados/sim/sim_irk_integrator.h
new file mode 100644
index 000000000..6851bacb3
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_irk_integrator.h
@@ -0,0 +1,186 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
+#define ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/types.h"
+
+#include "blasfeo/include/blasfeo_common.h"
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+
+} sim_irk_dims;
+
+
+
+typedef struct
+{
+    /* external functions */
+    // implicit fun - can either be fully implicit ode or dae
+    //          - i.e. dae has z as additional last argument & nz > 0
+    external_function_generic *impl_ode_fun;
+    // implicit ode & jac_x & jax_xdot & jac_z
+    external_function_generic *impl_ode_fun_jac_x_xdot_z;
+    // jax_x & jac_xdot & jac_u & jac_z of implicit ode
+    external_function_generic *impl_ode_jac_x_xdot_u_z;
+    // hessian of implicit ode:
+    external_function_generic *impl_ode_hess;
+} irk_model;
+
+
+
+typedef struct
+{
+    struct blasfeo_dvec *rG;        // residuals of G (nx*ns)
+    struct blasfeo_dvec *K;         // internal K variables ((nx+nz)*ns)
+    struct blasfeo_dvec *xt;        // temporary x
+    struct blasfeo_dvec *xn;        // x at each integration step
+    struct blasfeo_dvec xtdot;      // temporary xdot
+
+    struct blasfeo_dvec *lambda;    // adjoint sensitivities (nx + nu)
+    struct blasfeo_dvec *lambdaK;   // auxiliary variable ((nx+nz)*ns) for adjoint propagation
+
+    struct blasfeo_dmat df_dx;     // temporary Jacobian of ode w.r.t x (nx+nz, nx)
+    struct blasfeo_dmat df_dxdot;  // temporary Jacobian of ode w.r.t xdot (nx+nz, nx)
+    struct blasfeo_dmat df_du;     // temporary Jacobian of ode w.r.t u (nx+nz, nu)
+    struct blasfeo_dmat df_dz;     // temporary Jacobian of ode w.r.t z (nx+nz, nu)
+
+    /* NOTE: the memory allocation corresponding to the following fields is CONDITIONAL */
+
+    // only allocated if (opts->sens_algebraic || opts->output_z)
+    int *ipiv_one_stage;  // index of pivot vector (nx + nz)
+    double *Z_work;  // used to perform computations to get out->zn (ns)
+
+    // df_dxdotz, dk0_dxu, only allocated if (opts->sens_algebraic && opts->exact_z_output)
+    //      used for algebraic sensitivity generation
+    struct blasfeo_dmat df_dxdotz;  // temporary Jacobian of ode w.r.t. xdot,z (nx+nz, nx+nz);
+    struct blasfeo_dmat dk0_dxu;    // intermediate result, (nx+nz, nx+nu)
+
+    // dK_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
+    //         if ( opts->sens_hess) - array of (num_steps) blasfeo_dmat
+    //                                  to store intermediate results
+    struct blasfeo_dmat *dK_dxu;  // jacobian of (K,Z) over x and u ((nx+nz)*ns, nx+nu);
+
+    // S_forw: if (!opts->sens_hess) - single blasfeo_dmat that is reused
+    //         if ( opts->sens_hess) - array of (num_steps + 1) blasfeo_dmat
+    //                                  to store intermediate results
+    struct blasfeo_dmat *S_forw;  // forward sensitivities (nx, nx+nu)
+
+    // dG_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused
+    //         if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
+    struct blasfeo_dmat *dG_dxu;  // jacobian of G over x and u ((nx+nz)*ns, nx+nu)
+
+    // dG_dK:  if (!opts->sens_hess) - single blasfeo_dmat that is reused
+    //         if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results
+    struct blasfeo_dmat *dG_dK;   // jacobian of G over K ((nx+nz)*ns, (nx+nz)*ns)
+
+    // ipiv: index of pivot vector
+    //         if (!opts->sens_hess) - array (ns * (nx + nz)) that is reused
+    //         if ( opts->sens_hess) - array (ns * (nx + nz)) * num_steps, to store all
+    //              pivot vectors for dG_dxu
+    int *ipiv;  // index of pivot vector
+
+    // xn_traj, K_traj only available if( opts->sens_adj || opts->sens_hess )
+    struct blasfeo_dvec *xn_traj;  // xn trajectory
+    struct blasfeo_dvec *K_traj;   // K trajectory
+
+    /* the following variables are only available if (opts->sens_hess) */
+    // For Hessian propagation
+    struct blasfeo_dmat Hess;   // temporary Hessian (nx + nu, nx + nu)
+    // output of impl_ode_hess
+    struct blasfeo_dmat f_hess;  // size: (nx + nu, nx + nu)
+    struct blasfeo_dmat dxkzu_dw0;  // size (2*nx + nu + nz) x (nx + nu)
+    struct blasfeo_dmat tmp_dxkzu_dw0;  // size (2*nx + nu + nz) x (nx + nu)
+
+} sim_irk_workspace;
+
+
+typedef struct
+{
+    double *xdot;  // xdot[NX] - initialization for state derivatives k within the integrator
+    double *z;     // z[NZ] - initialization for algebraic variables z
+
+	double time_sim;
+	double time_ad;
+	double time_la;
+} sim_irk_memory;
+
+
+// get & set functions
+void sim_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
+void sim_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+// dims
+acados_size_t sim_irk_dims_calculate_size();
+void *sim_irk_dims_assign(void *config_, void *raw_memory);
+
+// model
+acados_size_t sim_irk_model_calculate_size(void *config, void *dims);
+void *sim_irk_model_assign(void *config, void *dims, void *raw_memory);
+int sim_irk_model_set(void *model, const char *field, void *value);
+
+// opts
+acados_size_t sim_irk_opts_calculate_size(void *config, void *dims);
+void *sim_irk_opts_assign(void *config, void *dims, void *raw_memory);
+void sim_irk_opts_initialize_default(void *config, void *dims, void *opts_);
+void sim_irk_opts_update(void *config_, void *dims, void *opts_);
+void sim_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
+
+// memory
+acados_size_t sim_irk_memory_calculate_size(void *config, void *dims, void *opts_);
+void *sim_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value);
+
+// workspace
+acados_size_t sim_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
+void sim_irk_config_initialize_default(void *config);
+
+// main
+int sim_irk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_IRK_INTEGRATOR_H_
diff --git a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
new file mode 100644
index 000000000..8e476b0e2
--- /dev/null
+++ b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h
@@ -0,0 +1,159 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
+#define ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/sim/sim_common.h"
+#include "acados/utils/types.h"
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+} sim_lifted_irk_dims;
+
+
+
+typedef struct
+{
+    /* external functions */
+    // implicit ode
+    external_function_generic *impl_ode_fun;
+    // implicit ode & jax_x & jac_xdot & jac_u implicit ode
+    external_function_generic *impl_ode_fun_jac_x_xdot_u;
+
+} lifted_irk_model;
+
+
+
+typedef struct
+{
+
+    struct blasfeo_dmat *J_temp_x;     // temporary Jacobian of ode w.r.t x (nx, nx)
+    struct blasfeo_dmat *J_temp_xdot;  // temporary Jacobian of ode w.r.t xdot (nx, nx)
+    struct blasfeo_dmat *J_temp_u;     // temporary Jacobian of ode w.r.t u (nx, nu)
+
+    struct blasfeo_dvec *rG;      // residuals of G (nx*ns)
+    struct blasfeo_dvec *xt;      // temporary x
+    struct blasfeo_dvec *xn;      // x at each integration step (for evaluations)
+    struct blasfeo_dvec *xn_out;  // x at each integration step (output)
+    struct blasfeo_dvec *dxn;     // dx at each integration step
+    struct blasfeo_dvec *w;       // stacked x and u
+
+    int *ipiv;  // index of pivot vector
+
+} sim_lifted_irk_workspace;
+
+
+
+typedef struct
+{
+    // memory for lifted integrators
+    struct blasfeo_dmat *S_forw;    // forward sensitivities
+    struct blasfeo_dmat *JGK;       // jacobian of G over K (nx*ns, nx*ns)
+    struct blasfeo_dmat *JGf;       // jacobian of G over x and u (nx*ns, nx+nu);
+    struct blasfeo_dmat *JKf;       // jacobian of K over x and u (nx*ns, nx+nu);
+
+    struct blasfeo_dvec *K;         // internal variables (nx*ns)
+    struct blasfeo_dvec *x;         // states (nx) -- for expansion step
+    struct blasfeo_dvec *u;         // controls (nu) -- for expansion step
+
+    int update_sens;
+
+	double time_sim;
+	double time_ad;
+	double time_la;
+
+} sim_lifted_irk_memory;
+
+
+
+/* dims */
+void sim_lifted_irk_dims_set(void *config_, void *dims_, const char *field, const int *value);
+void sim_lifted_irk_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+acados_size_t sim_lifted_irk_dims_calculate_size();
+//
+void *sim_lifted_irk_dims_assign(void* config_, void *raw_memory);
+
+/* model */
+//
+acados_size_t sim_lifted_irk_model_calculate_size(void *config, void *dims);
+//
+void *sim_lifted_irk_model_assign(void *config, void *dims, void *raw_memory);
+//
+int sim_lifted_irk_model_set(void *model_, const char *field, void *value);
+
+/* opts */
+//
+acados_size_t sim_lifted_irk_opts_calculate_size(void *config, void *dims);
+//
+void *sim_lifted_irk_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void sim_lifted_irk_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void sim_lifted_irk_opts_update(void *config_, void *dims, void *opts_);
+//
+void sim_lifted_irk_opts_set(void *config_, void *opts_, const char *field, void *value);
+
+/* memory */
+//
+acados_size_t sim_lifted_irk_memory_calculate_size(void *config, void *dims, void *opts_);
+//
+void *sim_lifted_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory);
+
+/* workspace */
+//
+acados_size_t sim_lifted_irk_workspace_calculate_size(void *config, void *dims, void *opts_);
+//
+void sim_lifted_irk_config_initialize_default(void *config);
+
+/* solver */
+//
+int sim_lifted_irk(void *config, sim_in *in, sim_out *out, void *opts_,
+        void *mem_, void *work_);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_
diff --git a/third_party/acados/include/acados/utils/external_function_generic.h b/third_party/acados/include/acados/utils/external_function_generic.h
new file mode 100644
index 000000000..021363f26
--- /dev/null
+++ b/third_party/acados/include/acados/utils/external_function_generic.h
@@ -0,0 +1,245 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
+#define ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/utils/types.h"
+
+/************************************************
+ * generic external function
+ ************************************************/
+
+// type of arguments
+typedef enum {
+    COLMAJ,
+    BLASFEO_DMAT,
+    BLASFEO_DVEC,
+    COLMAJ_ARGS,
+    BLASFEO_DMAT_ARGS,
+    BLASFEO_DVEC_ARGS,
+    IGNORE_ARGUMENT
+} ext_fun_arg_t;
+
+struct colmaj_args
+{
+    double *A;
+    int lda;
+};
+
+struct blasfeo_dmat_args
+{
+    struct blasfeo_dmat *A;
+    int ai;
+    int aj;
+};
+
+struct blasfeo_dvec_args
+{
+    struct blasfeo_dvec *x;
+    int xi;
+};
+
+// prototype of an external function
+typedef struct
+{
+    // public members (have to be before private ones)
+    void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
+    // private members
+    // .....
+} external_function_generic;
+
+
+
+/************************************************
+ * generic external parametric function
+ ************************************************/
+
+// prototype of a parametric external function
+typedef struct
+{
+    // public members for core (have to be before private ones)
+    void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
+	// public members for interfaces
+    void (*get_nparam)(void *, int *);
+    void (*set_param)(void *, double *);
+    void (*set_param_sparse)(void *, int n_update, int *idx, double *);
+    // private members
+    void *ptr_ext_mem;  // pointer to external memory
+    int (*fun)(void **, void **, void *);
+    double *p;  // parameters
+    int np;     // number of parameters
+    // .....
+} external_function_param_generic;
+
+//
+acados_size_t external_function_param_generic_struct_size();
+//
+void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value);
+//
+acados_size_t external_function_param_generic_calculate_size(external_function_param_generic *fun, int np);
+//
+void external_function_param_generic_assign(external_function_param_generic *fun, void *mem);
+//
+void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out);
+//
+void external_function_param_generic_get_nparam(void *self, int *np);
+//
+void external_function_param_generic_set_param(void *self, double *p);
+
+
+/************************************************
+ * casadi external function
+ ************************************************/
+
+typedef struct
+{
+    // public members (have to be the same as in the prototype, and before the private ones)
+    void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
+    // private members
+    void *ptr_ext_mem;  // pointer to external memory
+    int (*casadi_fun)(const double **, double **, int *, double *, void *);
+    int (*casadi_work)(int *, int *, int *, int *);
+    const int *(*casadi_sparsity_in)(int);
+    const int *(*casadi_sparsity_out)(int);
+    int (*casadi_n_in)();
+    int (*casadi_n_out)();
+    double **args;
+    double **res;
+    double *w;
+    int *iw;
+    int *args_size;     // size of args[i]
+    int *res_size;      // size of res[i]
+    int args_num;       // number of args arrays
+    int args_size_tot;  // total size of args arrays
+    int res_num;        // number of res arrays
+    int res_size_tot;   // total size of res arrays
+    int in_num;         // number of input arrays
+    int out_num;        // number of output arrays
+    int iw_size;        // number of ints for worksapce
+    int w_size;         // number of doubles for workspace
+} external_function_casadi;
+
+//
+acados_size_t external_function_casadi_struct_size();
+//
+void external_function_casadi_set_fun(external_function_casadi *fun, void *value);
+//
+void external_function_casadi_set_work(external_function_casadi *fun, void *value);
+//
+void external_function_casadi_set_sparsity_in(external_function_casadi *fun, void *value);
+//
+void external_function_casadi_set_sparsity_out(external_function_casadi *fun, void *value);
+//
+void external_function_casadi_set_n_in(external_function_casadi *fun, void *value);
+//
+void external_function_casadi_set_n_out(external_function_casadi *fun, void *value);
+//
+acados_size_t external_function_casadi_calculate_size(external_function_casadi *fun);
+//
+void external_function_casadi_assign(external_function_casadi *fun, void *mem);
+//
+void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
+                                      ext_fun_arg_t *type_out, void **out);
+
+/************************************************
+ * casadi external parametric function
+ ************************************************/
+
+typedef struct
+{
+    // public members for core (have to be the same as in the prototype, and before the private ones)
+    void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **);
+	// public members for interfaces
+    void (*get_nparam)(void *, int *);
+    void (*set_param)(void *, double *);
+    void (*set_param_sparse)(void *, int n_update, int *idx, double *);
+    // private members
+    void *ptr_ext_mem;  // pointer to external memory
+    int (*casadi_fun)(const double **, double **, int *, double *, void *);
+    int (*casadi_work)(int *, int *, int *, int *);
+    const int *(*casadi_sparsity_in)(int);
+    const int *(*casadi_sparsity_out)(int);
+    int (*casadi_n_in)();
+    int (*casadi_n_out)();
+    double **args;
+    double **res;
+    double *w;
+    int *iw;
+    int *args_size;     // size of args[i]
+    int *res_size;      // size of res[i]
+    int args_num;       // number of args arrays
+    int args_size_tot;  // total size of args arrays
+    int res_num;        // number of res arrays
+    int res_size_tot;   // total size of res arrays
+    int in_num;         // number of input arrays
+    int out_num;        // number of output arrays
+    int iw_size;        // number of ints for worksapce
+    int w_size;         // number of doubles for workspace
+    int np;             // number of parameters
+} external_function_param_casadi;
+
+//
+acados_size_t external_function_param_casadi_struct_size();
+//
+void external_function_param_casadi_set_fun(external_function_param_casadi *fun, void *value);
+//
+void external_function_param_casadi_set_work(external_function_param_casadi *fun, void *value);
+//
+void external_function_param_casadi_set_sparsity_in(external_function_param_casadi *fun, void *value);
+//
+void external_function_param_casadi_set_sparsity_out(external_function_param_casadi *fun, void *value);
+//
+void external_function_param_casadi_set_n_in(external_function_param_casadi *fun, void *value);
+//
+void external_function_param_casadi_set_n_out(external_function_param_casadi *fun, void *value);
+//
+acados_size_t external_function_param_casadi_calculate_size(external_function_param_casadi *fun, int np);
+//
+void external_function_param_casadi_assign(external_function_param_casadi *fun, void *mem);
+//
+void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in,
+                                            ext_fun_arg_t *type_out, void **out);
+//
+void external_function_param_casadi_get_nparam(void *self, int *np);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_
diff --git a/third_party/acados/include/acados/utils/math.h b/third_party/acados/include/acados/utils/math.h
new file mode 100644
index 000000000..ac0d3263e
--- /dev/null
+++ b/third_party/acados/include/acados/utils/math.h
@@ -0,0 +1,105 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+#ifndef ACADOS_UTILS_MATH_H_
+#define ACADOS_UTILS_MATH_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/utils/types.h"
+
+#if defined(__DSPACE__)
+double fmax(double a, double b);
+#endif
+
+#define MIN(a,b) (((a)<(b))?(a):(b))
+#define MAX(a,b) (((a)>(b))?(a):(b))
+
+void dgemm_nn_3l(int m, int n, int k, double *A, int lda, double *B, int ldb, double *C, int ldc);
+// void dgemv_n_3l(int m, int n, double *A, int lda, double *x, double *y);
+// void dgemv_t_3l(int m, int n, double *A, int lda, double *x, double *y);
+// void dcopy_3l(int n, double *x, int incx, double *y, int incy);
+void daxpy_3l(int n, double da, double *dx, double *dy);
+void dscal_3l(int n, double da, double *dx);
+double twonormv(int n, double *ptrv);
+
+/* copies a matrix into another matrix */
+void dmcopy(int row, int col, double *ptrA, int lda, double *ptrB, int ldb);
+
+/* solution of a system of linear equations */
+void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info);
+
+/* matrix exponential */
+void expm(int row, double *A);
+
+int idamax_3l(int n, double *x);
+
+void dswap_3l(int n, double *x, int incx, double *y, int incy);
+
+void dger_3l(int m, int n, double alpha, double *x, int incx, double *y, int incy, double *A,
+             int lda);
+
+void dgetf2_3l(int m, int n, double *A, int lda, int *ipiv, int *info);
+
+void dlaswp_3l(int n, double *A, int lda, int k1, int k2, int *ipiv);
+
+void dtrsm_l_l_n_u_3l(int m, int n, double *A, int lda, double *B, int ldb);
+
+void dgetrs_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb);
+
+void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info);
+
+double onenorm(int row, int col, double *ptrA);
+
+// double twonormv(int n, double *ptrv);
+
+void padeapprox(int m, int row, double *A);
+
+void expm(int row, double *A);
+
+// void d_compute_qp_size_ocp2dense_rev(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng,
+//                                      int *nvd, int *ned, int *nbd, int *ngd);
+
+void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double *e);
+
+double minimum_of_doubles(double *x, int n);
+
+void neville_algorithm(double xx, int n, double *x, double *Q, double *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_MATH_H_
diff --git a/third_party/acados/include/acados/utils/mem.h b/third_party/acados/include/acados/utils/mem.h
new file mode 100644
index 000000000..7b9efc5ed
--- /dev/null
+++ b/third_party/acados/include/acados/utils/mem.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_MEM_H_
+#define ACADOS_UTILS_MEM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdio.h>
+#include <stdbool.h>
+
+#include "types.h"
+
+// blasfeo
+#include "blasfeo/include/blasfeo_d_aux.h"
+#include "blasfeo/include/blasfeo_d_aux_ext_dep.h"
+
+// TODO(dimitris): probably does not belong here
+typedef struct
+{
+    int (*fun)(void *);
+    acados_size_t (*calculate_args_size)(void *);
+    void *(*assign_args)(void *);
+    void (*initialize_default_args)(void *);
+    acados_size_t (*calculate_memory_size)(void *);
+    void *(*assign_memory)(void *);
+    acados_size_t (*calculate_workspace_size)(void *);
+} module_solver;
+
+// make int counter of memory multiple of a number (typically 8 or 64)
+void make_int_multiple_of(acados_size_t num, acados_size_t *size);
+
+// align char pointer to number (typically 8 for pointers and doubles,
+// 64 for blasfeo structs) and return offset
+int align_char_to(int num, char **c_ptr);
+
+// switch between malloc and calloc (for valgrinding)
+void *acados_malloc(size_t nitems, acados_size_t size);
+
+// uses always calloc
+void *acados_calloc(size_t nitems, acados_size_t size);
+
+// allocate vector of pointers to vectors of doubles and advance pointer
+void assign_and_advance_double_ptrs(int n, double ***v, char **ptr);
+
+// allocate vector of pointers to vectors of ints and advance pointer
+void assign_and_advance_int_ptrs(int n, int ***v, char **ptr);
+
+// allocate vector of pointers to strvecs and advance pointer
+void assign_and_advance_blasfeo_dvec_structs(int n, struct blasfeo_dvec **sv, char **ptr);
+
+// allocate vector of pointers to strmats and advance pointer
+void assign_and_advance_blasfeo_dmat_structs(int n, struct blasfeo_dmat **sm, char **ptr);
+
+// allocate vector of pointers to vector of pointers to strmats and advance pointer
+void assign_and_advance_blasfeo_dmat_ptrs(int n, struct blasfeo_dmat ***sm, char **ptr);
+
+// allocate vector of chars and advance pointer
+void assign_and_advance_char(int n, char **v, char **ptr);
+
+// allocate vector of ints and advance pointer
+void assign_and_advance_int(int n, int **v, char **ptr);
+
+// allocate vector of bools and advance pointer
+void assign_and_advance_bool(int n, bool **v, char **ptr);
+
+// allocate vector of doubles and advance pointer
+void assign_and_advance_double(int n, double **v, char **ptr);
+
+// allocate strvec and advance pointer
+void assign_and_advance_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char **ptr);
+
+// allocate strmat and advance pointer
+void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_MEM_H_
diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h
new file mode 100644
index 000000000..2993447db
--- /dev/null
+++ b/third_party/acados/include/acados/utils/print.h
@@ -0,0 +1,109 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_PRINT_H_
+#define ACADOS_UTILS_PRINT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_qp/ocp_qp_common_frontend.h"
+#include "acados/utils/types.h"
+
+// void print_matrix(char *file_name, const real_t *matrix, const int_t nrows, const int_t ncols);
+
+// void print_matrix_name(char *file_name, char *name, const real_t *matrix, const int_t nrows,
+//                        const int_t ncols);
+
+// void print_int_matrix(char *file_name, const int_t *matrix, const int_t nrows, const int_t ncols);
+
+// void print_array(char *file_name, real_t *array, int_t size);
+
+// void print_int_array(char *file_name, const int_t *array, int_t size);
+
+void read_matrix(const char *file_name, real_t *array, const int_t nrows, const int_t ncols);
+
+void write_double_vector_to_txt(real_t *vec, int_t n, const char *fname);
+
+// ocp nlp
+// TODO(andrea): inconsistent naming
+void ocp_nlp_dims_print(ocp_nlp_dims *dims);
+// TODO(andrea): inconsistent naming
+void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
+// TODO(andrea): inconsistent naming
+void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res);
+
+// ocp qp
+void print_ocp_qp_dims(ocp_qp_dims *dims);
+
+// void print_dense_qp_dims(dense_qp_dims *dims);
+
+void print_ocp_qp_in(ocp_qp_in *qp_in);
+
+void print_ocp_qp_out(ocp_qp_out *qp_out);
+
+// void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in);
+
+// void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out);
+
+void print_ocp_qp_res(ocp_qp_res *qp_res);
+
+// void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp);
+
+// void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp);
+
+// void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out);
+
+void print_dense_qp_in(dense_qp_in *qp_in);
+
+void print_qp_info(qp_info *info);
+
+// void acados_warning(char warning_string[]);
+
+// void acados_error(char error_string[]);
+
+// void acados_not_implemented(char feature_string[]);
+
+// blasfeo
+// void print_blasfeo_target();
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_PRINT_H_
diff --git a/third_party/acados/include/acados/utils/strsep.h b/third_party/acados/include/acados/utils/strsep.h
new file mode 100644
index 000000000..02f183559
--- /dev/null
+++ b/third_party/acados/include/acados/utils/strsep.h
@@ -0,0 +1,72 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_STRSEP_H_
+#define ACADOS_UTILS_STRSEP_H_
+
+#ifdef __cplusplus
+#include <string>
+#define STD(x) std::x
+namespace std
+{
+#else
+#include <string.h>
+#define STD(x) x
+#endif
+
+char* strsep_acados(char** stringp, const char* delim)
+{
+    char* result;
+
+    if ((stringp == NULL) || (*stringp == NULL)) return NULL;
+
+    result = *stringp;
+
+    while (**stringp && !STD(strchr)(delim, **stringp)) ++*stringp;
+
+    if (**stringp)
+        *(*stringp)++ = '\0';
+    else
+        *stringp = NULL;
+
+    return result;
+}
+
+#ifdef __cplusplus
+}  // namespace std
+#endif
+
+#undef STD
+
+#endif  // ACADOS_UTILS_STRSEP_H_
diff --git a/third_party/acados/include/acados/utils/timing.h b/third_party/acados/include/acados/utils/timing.h
new file mode 100644
index 000000000..823b763f6
--- /dev/null
+++ b/third_party/acados/include/acados/utils/timing.h
@@ -0,0 +1,125 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_TIMING_H_
+#define ACADOS_UTILS_TIMING_H_
+
+#include "acados/utils/types.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifdef MEASURE_TIMINGS
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+/* Use Windows QueryPerformanceCounter for timing. */
+#include <Windows.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acados_timer_
+{
+    LARGE_INTEGER tic;
+    LARGE_INTEGER toc;
+    LARGE_INTEGER freq;
+} acados_timer;
+
+#elif defined(__APPLE__)
+
+#include <mach/mach_time.h>
+
+/** A structure for keeping internal timer data. */
+typedef struct acados_timer_
+{
+    uint64_t tic;
+    uint64_t toc;
+    mach_timebase_info_data_t tinfo;
+} acados_timer;
+
+#elif defined(__DSPACE__)
+
+#include <brtenv.h>
+
+typedef struct acados_timer_
+{
+    double time;
+} acados_timer;
+
+#else
+
+/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+#include <time.h>
+
+#if (__STDC_VERSION__ >= 199901L) && !(defined __MINGW32__ || defined __MINGW64__)  // C99 Mode
+
+#include <sys/stat.h>
+#include <sys/time.h>
+
+typedef struct acados_timer_
+{
+    struct timeval tic;
+    struct timeval toc;
+} acados_timer;
+
+#else  // ANSI C Mode
+
+/** A structure for keeping internal timer data. */
+typedef struct acados_timer_
+{
+    struct timespec tic;
+    struct timespec toc;
+} acados_timer;
+
+#endif  // __STDC_VERSION__ >= 199901L
+
+#endif  // (defined _WIN32 || defined _WIN64)
+
+#else
+
+// Dummy type when timings are off
+typedef real_t acados_timer;
+
+#endif  // MEASURE_TIMINGS
+
+/** A function for measurement of the current time. */
+void acados_tic(acados_timer* t);
+
+/** A function which returns the elapsed time. */
+real_t acados_toc(acados_timer* t);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_TIMING_H_
diff --git a/third_party/acados/include/acados/utils/types.h b/third_party/acados/include/acados/utils/types.h
new file mode 100644
index 000000000..99ad375fe
--- /dev/null
+++ b/third_party/acados/include/acados/utils/types.h
@@ -0,0 +1,79 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_UTILS_TYPES_H_
+#define ACADOS_UTILS_TYPES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stddef.h>
+
+#define MAX_STR_LEN 256
+#define ACADOS_EPS 1e-12
+#define ACADOS_NEG_INFTY -1.0e9
+#define ACADOS_POS_INFTY +1.0e9
+#define UNUSED(x) ((void)(x))
+
+
+
+typedef double real_t;
+typedef int int_t;
+typedef size_t acados_size_t;
+
+
+typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, double* w, void* mem);
+
+
+
+// enum of return values
+enum return_values
+{
+    ACADOS_SUCCESS,
+    ACADOS_FAILURE,
+    ACADOS_MAXITER,
+    ACADOS_MINSTEP,
+    ACADOS_QP_FAILURE,
+    ACADOS_READY,
+};
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_UTILS_TYPES_H_
diff --git a/third_party/acados/include/acados_c/condensing_interface.h b/third_party/acados/include/acados_c/condensing_interface.h
new file mode 100644
index 000000000..51fe82712
--- /dev/null
+++ b/third_party/acados/include/acados_c/condensing_interface.h
@@ -0,0 +1,84 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_
+#define INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_full_condensing.h"
+#include "acados/ocp_qp/ocp_qp_partial_condensing.h"
+
+typedef enum {
+    PARTIAL_CONDENSING,
+    FULL_CONDENSING,
+} condensing_t;
+
+typedef struct
+{
+    condensing_t condensing_type;
+} condensing_plan;
+
+typedef struct
+{
+    ocp_qp_xcond_config *config;
+    void *dims;
+    void *opts;
+    void *mem;
+    void *work;
+} condensing_module;
+
+ocp_qp_xcond_config *ocp_qp_condensing_config_create(condensing_plan *plan);
+//
+void *ocp_qp_condensing_opts_create(ocp_qp_xcond_config *config, void *dims_);
+//
+acados_size_t ocp_qp_condensing_calculate_size(ocp_qp_xcond_config *config, void *dims_, void *opts_);
+//
+condensing_module *ocp_qp_condensing_assign(ocp_qp_xcond_config *config, void *dims_,
+                                            void *opts_, void *raw_memory);
+//
+condensing_module *ocp_qp_condensing_create(ocp_qp_xcond_config *config, void *dims_,
+                                            void *opts_);
+//
+int ocp_qp_condense(condensing_module *module, void *qp_in, void *qp_out);
+//
+int ocp_qp_expand(condensing_module *module, void *qp_in, void *qp_out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_
diff --git a/third_party/acados/include/acados_c/dense_qp_interface.h b/third_party/acados/include/acados_c/dense_qp_interface.h
new file mode 100644
index 000000000..4ecc77381
--- /dev/null
+++ b/third_party/acados/include/acados_c/dense_qp_interface.h
@@ -0,0 +1,94 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_
+#define INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/dense_qp/dense_qp_common.h"
+
+typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP } dense_qp_solver_t;
+
+typedef struct
+{
+    dense_qp_solver_t qp_solver;
+} dense_qp_solver_plan;
+
+typedef struct
+{
+    qp_solver_config *config;
+    void *dims;
+    void *opts;
+    void *mem;
+    void *work;
+} dense_qp_solver;
+
+qp_solver_config *dense_qp_config_create(dense_qp_solver_plan *plan);
+//
+dense_qp_dims *dense_qp_dims_create();
+//
+dense_qp_in *dense_qp_in_create(qp_solver_config *config, dense_qp_dims *dims);
+//
+dense_qp_out *dense_qp_out_create(qp_solver_config *config, dense_qp_dims *dims);
+//
+void *dense_qp_opts_create(qp_solver_config *config, dense_qp_dims *dims);
+//
+acados_size_t dense_qp_calculate_size(qp_solver_config *config, dense_qp_dims *dims, void *opts_);
+//
+dense_qp_solver *dense_qp_assign(qp_solver_config *config, dense_qp_dims *dims, void *opts_,
+                                 void *raw_memory);
+//
+dense_qp_solver *dense_qp_create(qp_solver_config *config, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_solve(dense_qp_solver *solver, dense_qp_in *qp_in, dense_qp_out *qp_out);
+//
+void dense_qp_inf_norm_residuals(dense_qp_dims *dims, dense_qp_in *qp_in, dense_qp_out *qp_out,
+                                 double *res);
+//
+bool dense_qp_set_field_double_array(const char *field, double *arr, dense_qp_in *qp_in);
+//
+bool dense_qp_set_field_int_array(const char *field, int *arr, dense_qp_in *qp_in);
+//
+bool dense_qp_get_field_double_array(const char *field, dense_qp_in *qp_in, double *arr);
+//
+bool dense_qp_get_field_int_array(const char *field, dense_qp_in *qp_in, int *arr);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_
diff --git a/third_party/acados/include/acados_c/external_function_interface.h b/third_party/acados/include/acados_c/external_function_interface.h
new file mode 100644
index 000000000..683897507
--- /dev/null
+++ b/third_party/acados/include/acados_c/external_function_interface.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_
+#define INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/utils/external_function_generic.h"
+
+
+
+/************************************************
+ * generic external parametric function
+ ************************************************/
+
+//
+void external_function_param_generic_create(external_function_param_generic *fun, int np);
+//
+void external_function_param_generic_free(external_function_param_generic *fun);
+
+
+
+/************************************************
+ * casadi external function
+ ************************************************/
+
+//
+void external_function_casadi_create(external_function_casadi *fun);
+//
+void external_function_casadi_free(external_function_casadi *fun);
+//
+void external_function_casadi_create_array(int size, external_function_casadi *funs);
+//
+void external_function_casadi_free_array(int size, external_function_casadi *funs);
+
+
+
+/************************************************
+ * casadi external parametric function
+ ************************************************/
+
+//
+void external_function_param_casadi_create(external_function_param_casadi *fun, int np);
+//
+void external_function_param_casadi_free(external_function_param_casadi *fun);
+//
+void external_function_param_casadi_create_array(int size, external_function_param_casadi *funs,
+                                                 int np);
+//
+void external_function_param_casadi_free_array(int size, external_function_param_casadi *funs);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_
diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h
new file mode 100644
index 000000000..0f250a662
--- /dev/null
+++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h
@@ -0,0 +1,406 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_
+#define INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h"
+#include "acados/sim/sim_erk_integrator.h"
+#include "acados/sim/sim_irk_integrator.h"
+#include "acados/sim/sim_lifted_irk_integrator.h"
+#include "acados/sim/sim_gnsf.h"
+// acados_c
+#include "acados_c/ocp_qp_interface.h"
+#include "acados_c/sim_interface.h"
+
+
+/// Solution methods for optimal control problems.
+typedef enum
+{
+    SQP,
+    SQP_RTI,
+    INVALID_NLP_SOLVER,
+} ocp_nlp_solver_t;
+
+
+/// Types of the cost function.
+typedef enum
+{
+    LINEAR_LS,
+    NONLINEAR_LS,
+    EXTERNAL,
+    INVALID_COST,
+} ocp_nlp_cost_t;
+
+
+/// Types of the system dynamics, discrete or continuous time.
+typedef enum
+{
+    CONTINUOUS_MODEL,
+    DISCRETE_MODEL,
+    INVALID_DYNAMICS,
+} ocp_nlp_dynamics_t;
+
+
+/// Constraint types
+typedef enum
+{
+    /// Comprises simple bounds, polytopic constraints,
+    /// general non-linear constraints.
+    BGH,
+
+    /// Comprises simple bounds, polytopic constraints,
+    /// general non-linear constraints, and positive definite constraints.
+    BGP,
+
+    INVALID_CONSTRAINT,
+} ocp_nlp_constraints_t;
+
+
+/// Regularization types
+typedef enum
+{
+    NO_REGULARIZE,
+    MIRROR,
+    PROJECT,
+    PROJECT_REDUC_HESS,
+    CONVEXIFY,
+    INVALID_REGULARIZE,
+} ocp_nlp_reg_t;
+
+
+/// Structure to store the configuration of a non-linear program
+typedef struct ocp_nlp_plan
+{
+    /// QP solver configuration.
+    ocp_qp_solver_plan ocp_qp_solver_plan;
+
+    /// Simulation solver configuration for each stage.
+    sim_solver_plan *sim_solver_plan;
+
+    /// Nlp solver type.
+    ocp_nlp_solver_t nlp_solver;
+
+    /// Regularization type, defaults to no regularization.
+    ocp_nlp_reg_t regularization;
+
+    /// Cost type for each stage.
+    ocp_nlp_cost_t *nlp_cost;
+
+    /// Dynamics type for each stage.
+    ocp_nlp_dynamics_t *nlp_dynamics;
+
+    /// Constraints type for each stage.
+    ocp_nlp_constraints_t *nlp_constraints;
+
+    /// Horizon length.
+    int N;
+
+} ocp_nlp_plan;
+
+
+/// Structure to store the state/configuration for the non-linear programming solver
+typedef struct ocp_nlp_solver
+{
+    ocp_nlp_config *config;
+    void *dims;
+    void *opts;
+    void *mem;
+    void *work;
+} ocp_nlp_solver;
+
+
+/// Constructs an empty plan struct (user nlp configuration), all fields are set to a
+/// default/invalid state.
+///
+/// \param N Horizon length
+ocp_nlp_plan *ocp_nlp_plan_create(int N);
+
+/// Destructor for plan struct, frees memory.
+///
+/// \param plan_ The plan struct to destroy.
+void ocp_nlp_plan_destroy(void* plan_);
+
+
+/// Constructs an nlp configuration struct from a plan.
+///
+/// \param plan The plan (user nlp configuration).
+ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan);
+
+/// Desctructor of the nlp configuration.
+///
+/// \param config_ The configuration struct.
+void ocp_nlp_config_destroy(void *config_);
+
+
+/// Constructs an struct that contains the dimensions of the variables.
+///
+/// \param config_ The configuration struct.
+ocp_nlp_dims *ocp_nlp_dims_create(void *config_);
+
+/// Destructor of The dimension struct.
+///
+/// \param dims_ The dimension struct.
+void ocp_nlp_dims_destroy(void *dims_);
+
+
+/// Constructs an input struct for a non-linear programs.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+ocp_nlp_in *ocp_nlp_in_create(ocp_nlp_config *config, ocp_nlp_dims *dims);
+
+/// Destructor of the inputs struct.
+///
+/// \param in The inputs struct.
+void ocp_nlp_in_destroy(void *in);
+
+
+/// Sets the sampling times for the given stage.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param in The inputs struct.
+/// \param stage Stage number.
+/// \param field Has to be "Ts" (TBC other options).
+/// \param value The sampling times (floating point).
+void ocp_nlp_in_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage,
+        const char *field, void *value);
+
+
+/// Sets the function pointers to the dynamics functions for the given stage.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param in The inputs struct.
+/// \param stage Stage number.
+/// \param fun_type The name of the function type, either impl_ode_fun,
+///     impl_ode_fun_jac_x_xdot, impl_ode_jac_x_xdot_u (TBC)
+/// \param fun_ptr Function pointer to the dynamics function.
+int ocp_nlp_dynamics_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+        int stage, const char *fun_type, void *fun_ptr);
+
+
+/// Sets the function pointers to the cost functions for the given stage.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param in The inputs struct.
+/// \param stage Stage number.
+/// \param field The name of the field, either nls_res_jac,
+///     y_ref, W (others TBC)
+/// \param value Cost values.
+int ocp_nlp_cost_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+        int start_stage, int end_stage, const char *field, void *value, int dim);
+int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+        int start_stage, const char *field, void *value);
+
+
+/// Sets the function pointers to the constraints functions for the given stage.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param in The inputs struct.
+/// \param stage Stage number.
+/// \param field The name of the field, either lb, ub (others TBC)
+/// \param value Constraints function or values.
+int ocp_nlp_constraints_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+        int start_stage, int end_stage, const char *field, void *value, int dim);
+int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims,
+        ocp_nlp_in *in, int stage, const char *field, void *value);
+
+/* out */
+
+/// Constructs an output struct for the non-linear program.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+ocp_nlp_out *ocp_nlp_out_create(ocp_nlp_config *config, ocp_nlp_dims *dims);
+
+/// Destructor of the output struct.
+///
+/// \param out The output struct.
+void ocp_nlp_out_destroy(void *out);
+
+
+/// Sets fields in the output struct of an nlp solver, used to initialize the solver.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param out The output struct.
+/// \param stage Stage number.
+/// \param field The name of the field, either x, u, pi.
+/// \param value Initialization values.
+void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, void *value);
+
+
+/// Gets values of fields in the output struct of an nlp solver.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param out The output struct.
+/// \param stage Stage number.
+/// \param field The name of the field, either x, u, z, pi.
+/// \param value Pointer to the output memory.
+void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, void *value);
+
+void ocp_nlp_out_get_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int start_stage, int end_stage, const char *field, void *value);
+
+//
+void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
+        int stage, const char *field, void *value);
+
+// TODO(andrea): remove this once/if the MATLAB interface uses the new setters below?
+int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field);
+
+void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, int *dims_out);
+
+void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
+        int stage, const char *field, int *dims_out);
+
+
+/* opts */
+
+/// Creates an options struct for the non-linear program.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+void *ocp_nlp_solver_opts_create(ocp_nlp_config *config, ocp_nlp_dims *dims);
+
+/// Destructor of the options.
+///
+/// \param opts The options struct.
+void ocp_nlp_solver_opts_destroy(void *opts);
+
+/// Sets an option.
+///
+/// \param config The configuration struct.
+/// \param opts_ The options struct.
+/// \param field Name of the option.
+/// \param value Value of the option.
+void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value);
+
+
+void ocp_nlp_solver_opts_set_at_stage(ocp_nlp_config *config, void *opts_, int stage, const char *field, void* value);
+
+
+/// TBC
+/// Updates the options.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param opts_ The options struct.
+void ocp_nlp_solver_opts_update(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_);
+
+
+/* solver */
+
+/// Creates an ocp solver.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param opts_ The options struct.
+/// \return The solver.
+ocp_nlp_solver *ocp_nlp_solver_create(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_);
+
+/// Destructor of the solver.
+///
+/// \param solver The solver struct.
+void ocp_nlp_solver_destroy(void *solver);
+
+/// Solves the optimal control problem. Call ocp_nlp_precompute before
+/// calling this functions (TBC).
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+int ocp_nlp_solve(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+/// Performs precomputations for the solver. Needs to be called before
+/// ocl_nlp_solve (TBC).
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+
+/// Computes cost function value.
+///
+/// \param solver The solver struct.
+/// \param nlp_in The inputs struct.
+/// \param nlp_out The output struct.
+void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
+
+
+//
+void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
+
+/* get */
+/// \param config The configuration struct.
+/// \param solver The solver struct.
+/// \param field Supports "sqp_iter", "status", "nlp_res", "time_tot", ...
+/// \param return_value_ Pointer to the output memory.
+void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver,
+        const char *field, void *return_value_);
+
+/* set */
+/// Sets the initial guesses for the integrator for the given stage.
+///
+/// \param config The configuration struct.
+/// \param solver The ocp_nlp_solver struct.
+/// \param stage Stage number.
+/// \param field Supports "z_guess", "xdot_guess" (IRK), "phi_guess" (GNSF-IRK)
+/// \param value The initial guess for the algebraic variables in the integrator (if continuous model is used).
+void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver,
+        int stage, const char *field, void *value);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_
diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h
new file mode 100644
index 000000000..a567ebeb3
--- /dev/null
+++ b/third_party/acados/include/acados_c/ocp_qp_interface.h
@@ -0,0 +1,261 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_
+#define INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
+
+
+/// QP solver types (Enumeration).
+///
+/// Full list of fields:
+///   PARTIAL_CONDENSING_HPIPM
+///   PARTIAL_CONDENSING_HPMPC
+///   PARTIAL_CONDENSING_OOQP
+///   PARTIAL_CONDENSING_OSQP
+///   PARTIAL_CONDENSING_QPDUNES
+///   FULL_CONDENSING_HPIPM
+///   FULL_CONDENSING_QPOASES
+///   FULL_CONDENSING_QORE
+///   FULL_CONDENSING_OOQP
+///   INVALID_QP_SOLVER
+///
+/// Note: In this enumeration the partial condensing solvers have to be
+///       specified before the full condensing solvers.
+typedef enum {
+    PARTIAL_CONDENSING_HPIPM,
+#ifdef ACADOS_WITH_HPMPC
+    PARTIAL_CONDENSING_HPMPC,
+#else
+    PARTIAL_CONDENSING_HPMPC_NOT_AVAILABLE,
+#endif
+#ifdef ACADOS_WITH_OOQP
+    PARTIAL_CONDENSING_OOQP,
+#else
+    PARTIAL_CONDENSING_OOQP_NOT_AVAILABLE,
+#endif
+#ifdef ACADOS_WITH_OSQP
+    PARTIAL_CONDENSING_OSQP,
+#else
+    PARTIAL_CONDENSING_OSQP_NOT_AVAILABLE,
+#endif
+#ifdef ACADOS_WITH_QPDUNES
+    PARTIAL_CONDENSING_QPDUNES,
+#else
+    PARTIAL_CONDENSING_QPDUNES_NOT_AVAILABLE,
+#endif
+    FULL_CONDENSING_HPIPM,
+#ifdef ACADOS_WITH_QPOASES
+    FULL_CONDENSING_QPOASES,
+#else
+    FULL_CONDENSING_QPOASES_NOT_AVAILABLE,
+#endif
+#ifdef ACADOS_WITH_QORE
+    FULL_CONDENSING_QORE,
+#else
+    FULL_CONDENSING_QORE_NOT_AVAILABLE,
+#endif
+#ifdef ACADOS_WITH_OOQP
+    FULL_CONDENSING_OOQP,
+#else
+    FULL_CONDENSING_OOQP_NOT_AVAILABLE,
+#endif
+    INVALID_QP_SOLVER,
+} ocp_qp_solver_t;
+
+
+/// Struct containing qp solver
+typedef struct
+{
+    ocp_qp_solver_t qp_solver;
+} ocp_qp_solver_plan;
+
+
+/// Linear ocp configuration.
+typedef struct
+{
+    ocp_qp_xcond_solver_config *config;
+    ocp_qp_xcond_solver_dims *dims;
+    void *opts;
+    void *mem;
+    void *work;
+} ocp_qp_solver;
+
+
+/// Initializes the qp solver configuration.
+/// TBC should this be private/static - no, used in ocp_nlp
+void ocp_qp_xcond_solver_config_initialize_from_plan(
+    ocp_qp_solver_t solver_name, ocp_qp_xcond_solver_config *solver_config);
+
+/// Constructs a qp solver config and Initializes with default values.
+///
+/// \param plan The qp solver plan struct.
+ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan plan);
+
+/// Destructor for config struct, frees memory.
+///
+/// \param config The config object to destroy.
+void ocp_qp_xcond_solver_config_free(ocp_qp_xcond_solver_config *config);
+
+
+/// Constructs a struct that contains the dimensions for the variables of the qp.
+///
+/// \param N The number of variables.
+ocp_qp_dims *ocp_qp_dims_create(int N);
+
+/// Destructor of The dimension struct.
+///
+/// \param dims The dimension struct.
+void ocp_qp_dims_free(void *dims);
+
+//
+ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create(ocp_qp_xcond_solver_config *config, int N);
+//
+ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create_from_ocp_qp_dims(
+    ocp_qp_xcond_solver_config *config, ocp_qp_dims *dims);
+//
+void ocp_qp_xcond_solver_dims_free(ocp_qp_xcond_solver_dims *dims_);
+
+void ocp_qp_xcond_solver_dims_set(void *config_, ocp_qp_xcond_solver_dims *dims,
+                                  int stage, const char *field, int* value);
+
+
+/// Constructs an input object for the qp.
+///
+/// \param dims The dimension struct.
+ocp_qp_in *ocp_qp_in_create(ocp_qp_dims *dims);
+
+
+void ocp_qp_in_set(ocp_qp_xcond_solver_config *config, ocp_qp_in *in,
+                   int stage, char *field, void *value);
+
+/// Destructor of the inputs struct.
+///
+/// \param in_ The inputs struct.
+void ocp_qp_in_free(void *in_);
+
+
+/// Constructs an outputs object for the qp.
+///
+/// \param dims The dimension struct.
+ocp_qp_out *ocp_qp_out_create(ocp_qp_dims *dims);
+
+/// Destructor of the output struct.
+///
+/// \param out_ The output struct.
+void ocp_qp_out_free(void *out_);
+
+
+/// Getter of output struct
+void ocp_qp_out_get(ocp_qp_out *out, const char *field, void *value);
+
+
+/// Constructs an options object for the qp.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+void *ocp_qp_xcond_solver_opts_create(ocp_qp_xcond_solver_config *config,
+                                      ocp_qp_xcond_solver_dims *dims);
+
+/// Destructor of the options struct.
+///
+/// \param opts The options struct to destroy.
+void ocp_qp_xcond_solver_opts_free(ocp_qp_xcond_solver_opts *opts);
+
+
+/// Setter of the options struct.
+///
+/// \param opts The options struct.
+void ocp_qp_xcond_solver_opts_set(ocp_qp_xcond_solver_config *config,
+           ocp_qp_xcond_solver_opts *opts, const char *field, void* value);
+
+/// TBC Should be private/static?
+acados_size_t ocp_qp_calculate_size(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims, void *opts_);
+
+
+/// TBC Reserves memory? TBC Should this be private?
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param opts_ The options struct.
+/// \param raw_memory Pointer to raw memory to assign to qp solver.
+ocp_qp_solver *ocp_qp_assign(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims,
+                             void *opts_, void *raw_memory);
+
+/// Creates a qp solver. Reserves memory.
+///
+/// \param config The configuration struct.
+/// \param dims The dimension struct.
+/// \param opts_ The options struct.
+ocp_qp_solver *ocp_qp_create(ocp_qp_xcond_solver_config *config,
+                             ocp_qp_xcond_solver_dims *dims, void *opts_);
+
+
+/// Destroys a qp solver. Frees memory.
+///
+/// \param solver The qp solver
+void ocp_qp_solver_destroy(ocp_qp_solver *solver);
+
+void ocp_qp_x_cond_solver_free(ocp_qp_xcond_solver_config *config,
+                             ocp_qp_xcond_solver_dims *dims, void *opts_);
+
+
+/// Solves the qp.
+///
+/// \param solver The solver.
+/// \param qp_in The inputs struct.
+/// \param qp_out The output struct.
+int ocp_qp_solve(ocp_qp_solver *solver, ocp_qp_in *qp_in, ocp_qp_out *qp_out);
+
+
+/// Calculates the infinity norm of the residuals.
+///
+/// \param dims The dimension struct.
+/// \param qp_in The inputs struct.
+/// \param qp_out The output struct.
+/// \param res Output array for the residuals.
+void ocp_qp_inf_norm_residuals(ocp_qp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out,
+                               double *res);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_
diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h
new file mode 100644
index 000000000..ba2e67bd6
--- /dev/null
+++ b/third_party/acados/include/acados_c/sim_interface.h
@@ -0,0 +1,140 @@
+/*
+ * Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
+ * Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
+ * Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
+ * Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef INTERFACES_ACADOS_C_SIM_INTERFACE_H_
+#define INTERFACES_ACADOS_C_SIM_INTERFACE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/sim/sim_common.h"
+
+
+
+typedef enum
+{
+	ERK,
+	IRK,
+	GNSF,
+	LIFTED_IRK,
+	INVALID_SIM_SOLVER,
+} sim_solver_t;
+
+
+
+typedef struct
+{
+    sim_solver_t sim_solver;
+} sim_solver_plan;
+
+
+
+typedef struct
+{
+    sim_config *config;
+    void *dims;
+    void *opts;
+    void *mem;
+    void *work;
+} sim_solver;
+
+
+
+/* config */
+//
+sim_config *sim_config_create(sim_solver_plan plan);
+//
+void sim_config_destroy(void *config);
+
+/* dims */
+//
+void *sim_dims_create(void *config_);
+//
+void sim_dims_destroy(void *dims);
+//
+void sim_dims_set(sim_config *config, void *dims, const char *field, const int* value);
+//
+void sim_dims_get(sim_config *config, void *dims, const char *field, int* value);
+//
+void sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, int *dims_out);
+
+/* in */
+//
+sim_in *sim_in_create(sim_config *config, void *dims);
+//
+void sim_in_destroy(void *out);
+//
+int sim_in_set(void *config_, void *dims_, sim_in *in, const char *field, void *value);
+
+
+/* out */
+//
+sim_out *sim_out_create(sim_config *config, void *dims);
+//
+void sim_out_destroy(void *out);
+//
+int sim_out_get(void *config, void *dims, sim_out *out, const char *field, void *value);
+
+/* opts */
+//
+void *sim_opts_create(sim_config *config, void *dims);
+//
+void sim_opts_destroy(void *opts);
+//
+void sim_opts_set(sim_config *config, void *opts, const char *field, void *value);
+//
+void sim_opts_get(sim_config *config, void *opts, const char *field, void *value);
+
+/* solver */
+//
+acados_size_t sim_calculate_size(sim_config *config, void *dims, void *opts_);
+//
+sim_solver *sim_assign(sim_config *config, void *dims, void *opts_, void *raw_memory);
+//
+sim_solver *sim_solver_create(sim_config *config, void *dims, void *opts_);
+//
+void sim_solver_destroy(void *solver);
+//
+int sim_solve(sim_solver *solver, sim_in *in, sim_out *out);
+//
+int sim_precompute(sim_solver *solver, sim_in *in, sim_out *out);
+//
+int sim_solver_set(sim_solver *solver, const char *field, void *value);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // INTERFACES_ACADOS_C_SIM_INTERFACE_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo.h b/third_party/acados/include/blasfeo/include/blasfeo.h
new file mode 100644
index 000000000..c85491819
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo.h
@@ -0,0 +1,52 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#include "blasfeo_processor_features.h"
+#include "blasfeo_target.h"
+#include "blasfeo_block_size.h"
+#include "blasfeo_stdlib.h"
+#include "blasfeo_common.h"
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_aux_ext_dep.h"
+#include "blasfeo_d_kernel.h"
+#include "blasfeo_d_blas.h"
+#include "blasfeo_s_aux.h"
+#include "blasfeo_s_aux_ext_dep.h"
+#include "blasfeo_s_kernel.h"
+#include "blasfeo_s_blas.h"
+#include "blasfeo_i_aux_ext_dep.h"
+#include "blasfeo_v_aux_ext_dep.h"
+#include "blasfeo_timing.h"
+#include "blasfeo_memory.h"
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_block_size.h b/third_party/acados/include/blasfeo/include/blasfeo_block_size.h
new file mode 100644
index 000000000..182cc71b1
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_block_size.h
@@ -0,0 +1,371 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_BLOCK_SIZE_H_
+#define BLASFEO_BLOCK_SIZE_H_
+
+
+
+#define D_EL_SIZE 8 // double precision
+#define S_EL_SIZE 4 // single precision
+
+
+
+#if defined( TARGET_X64_INTEL_HASWELL )
+// common
+#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way
+#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB
+#define LLC_CACHE_SIZE (6*1024*1024) // LLC cache size: 6 MB ; TLB 1024*4 kB = 4 MB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 12 // max kernel size
+#define D_KC 256 // 192
+#define D_NC 72 //96 //72 // 120 // 512
+#define D_MC 1500 // 6000
+// single
+#define S_PS 8 // panel size
+#define S_PLD 4 // 2 // GCD of panel length
+#define S_M_KERNEL 24 // max kernel size
+#define S_KC 256
+#define S_NC 144
+#define S_MC 3000
+
+#elif defined( TARGET_X64_INTEL_SANDY_BRIDGE )
+// common
+#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way
+#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB
+#define LLC_CACHE_SIZE (4*1024*1024) // LLC cache size: 4 MB ; TLB 1024*4 kB = 4 MB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 8 // max kernel size
+#define D_KC 256 //320 //256 //320
+#define D_NC 72 //64 //72 //60 // 120
+#define D_MC 1000 // 800
+// single
+#define S_PS 8 // panel size
+#define S_PLD 4 // 2 // GCD of panel length
+#define S_M_KERNEL 16 // max kernel size
+#define S_KC 256
+#define S_NC 144
+#define S_MC 2000
+
+#elif defined( TARGET_X64_INTEL_CORE )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+#elif defined( TARGET_X64_AMD_BULLDOZER )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_X86_AMD_JAGUAR )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_X86_AMD_BARCELONA )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined(TARGET_ARMV8A_ARM_CORTEX_A76)
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (64*1024) // L1 data cache size: 64 kB, 4-way ; DTLB1 48*4 kB = 192 kB
+#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 8 // max kernel size
+#define D_KC 512 //256
+#define D_NC 128 //256
+#define D_MC 6000
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 8 // max kernel size
+#define S_KC 512
+#define S_NC 256
+#define S_MC 6000
+
+
+#elif defined(TARGET_ARMV8A_ARM_CORTEX_A73)
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 (64?) kB, 4-way, seen as 8-(16-)way ; DTLB1 48*4 kB = 192 kB
+#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 8 // max kernel size
+#define D_KC 320
+#define D_NC 256
+#define D_MC 6000
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 8 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined(TARGET_ARMV8A_ARM_CORTEX_A57)
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 2-way ; DTLB1 32*4 kB = 128 kB
+#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB // 2 MB ???
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 8 // max kernel size
+#define D_KC 224 //256 //192
+#define D_NC 40 //36 //48
+#define D_MC 512 //488 //600
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 8 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined(TARGET_ARMV8A_ARM_CORTEX_A55)
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ; DTLB1 16*4 kB = 64 kB
+#define LLC_CACHE_SIZE (512*1024) // LLC cache size: 512 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 12 // max kernel size
+#define D_KC 224
+#define D_NC 160
+#define D_MC 6000
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 8 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined(TARGET_ARMV8A_ARM_CORTEX_A53)
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ??? ; DTLB1 10*4 kB = 40 kB
+#define LLC_CACHE_SIZE (256*1024) // LLC cache size: 256 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 12 // max kernel size
+#define D_KC 160
+#define D_NC 128
+#define D_MC 6000
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 8 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_ARMV7A_ARM_CORTEX_A15 )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_ARMV7A_ARM_CORTEX_A7 )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_ARMV7A_ARM_CORTEX_A9 )
+// common
+#define CACHE_LINE_SIZE 32
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#elif defined( TARGET_GENERIC )
+// common
+#define CACHE_LINE_SIZE 64
+#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB
+// double
+#define D_PS 4 // panel size
+#define D_PLD 4 // 2 // GCD of panel length
+#define D_M_KERNEL 4 // max kernel size
+#define D_KC 256
+#define D_NC 128 // TODO these are just dummy
+#define D_MC 3000 // TODO these are just dummy
+
+// single
+#define S_PS 4
+#define S_PLD 4 //2
+#define S_M_KERNEL 4 // max kernel size
+#define S_KC 256
+#define S_NC 128 // TODO these are just dummy
+#define S_MC 3000 // TODO these are just dummy
+
+
+#else
+#error "Unknown architecture"
+#endif
+
+
+
+#define D_CACHE_LINE_EL (CACHE_LINE_SIZE/D_EL_SIZE)
+#define D_L1_CACHE_EL (L1_CACHE_SIZE/D_EL_SIZE)
+#define D_L2_CACHE_EL (L2_CACHE_SIZE/D_EL_SIZE)
+#define D_LLC_CACHE_EL (LLC_CACHE_SIZE/D_EL_SIZE)
+
+#define S_CACHE_LINE_EL (CACHE_LINE_SIZE/S_EL_SIZE)
+#define S_L1_CACHE_EL (L1_CACHE_SIZE/S_EL_SIZE)
+#define S_L2_CACHE_EL (L2_CACHE_SIZE/S_EL_SIZE)
+#define S_LLC_CACHE_EL (LLC_CACHE_SIZE/S_EL_SIZE)
+
+
+
+#endif  // BLASFEO_BLOCK_SIZE_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_common.h b/third_party/acados/include/blasfeo/include/blasfeo_common.h
new file mode 100644
index 000000000..a648142ba
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_common.h
@@ -0,0 +1,236 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_COMMON_H_
+#define BLASFEO_COMMON_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#if defined(__GNUC__) || defined(__clang__) || defined(__INTEL_COMPILER) || defined(__ICL) || defined(__ICC) || defined(__INTEL_LLVM_COMPILER)
+#define ALIGNED(VEC, BYTES) VEC __attribute__ ((aligned ( BYTES )))
+#elif defined (_MSC_VER)
+#define ALIGNED(VEC, BYTES) __declspec(align( BYTES )) VEC
+#else
+#define ALIGNED(VEC, BYTES) VEC
+#endif
+
+
+
+
+#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_PANELMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_PANELMAJ) )
+
+#include "blasfeo_block_size.h"
+
+// matrix structure
+struct blasfeo_dmat
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size
+	double *dA; // pointer to a min(m,n) (or max???) array of doubles
+	int m; // rows
+	int n; // cols
+	int pm; // packed number or rows
+	int cn; // packed number or cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_smat
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size
+	float *dA; // pointer to a min(m,n) (or max???) array of floats
+	int m; // rows
+	int n; // cols
+	int pm; // packed number or rows
+	int cn; // packed number or cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+// vector structure
+struct blasfeo_dvec
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pa; // pointer to a pm array of doubles, the first is aligned to cache line size
+	int m; // size
+	int pm; // packed size
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_svec
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pa; // pointer to a pm array of floats, the first is aligned to cache line size
+	int m; // size
+	int pm; // packed size
+	int memsize; // size of needed memory
+	};
+
+#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(D_PS-1)))*(sA)->cn+(aj)*D_PS+((ai)&(D_PS-1))])
+#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(S_PS-1)))*(sA)->cn+(aj)*S_PS+((ai)&(S_PS-1))])
+#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai])
+#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai])
+
+#elif ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_COLMAJ) ) | defined(LA_EXTERNAL_BLAS_WRAPPER)
+
+// matrix structure
+struct blasfeo_dmat
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pA; // pointer to a m*n array of doubles
+	double *dA; // pointer to a min(m,n) (or max???) array of doubles
+	int m; // rows
+	int n; // cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_smat
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pA; // pointer to a m*n array of floats
+	float *dA; // pointer to a min(m,n) (or max???) array of floats
+	int m; // rows
+	int n; // cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+// vector structure
+struct blasfeo_dvec
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pa; // pointer to a m array of doubles, the first is aligned to cache line size
+	int m; // size
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_svec
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pa; // pointer to a m array of floats, the first is aligned to cache line size
+	int m; // size
+	int memsize; // size of needed memory
+	};
+
+#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m])
+#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m])
+#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai])
+#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai])
+
+#else
+
+#error : wrong LA or MF choice
+
+#endif
+
+
+
+// Explicitly panel-major matrix structure
+struct blasfeo_pm_dmat
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size
+	double *dA; // pointer to a min(m,n) (or max???) array of doubles
+	int m; // rows
+	int n; // cols
+	int pm; // packed number or rows
+	int cn; // packed number or cols
+	int use_dA; // flag to tell if dA can be used
+	int ps; // panel size
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_pm_smat
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size
+	float *dA; // pointer to a min(m,n) (or max???) array of floats
+	int m; // rows
+	int n; // cols
+	int pm; // packed number or rows
+	int cn; // packed number or cols
+	int use_dA; // flag to tell if dA can be used
+	int ps; // panel size
+	int memsize; // size of needed memory
+	};
+
+// Explicitly column-major matrix structure
+struct blasfeo_cm_dmat
+	{
+	double *mem; // pointer to passed chunk of memory
+	double *pA; // pointer to a m*n array of doubles
+	double *dA; // pointer to a min(m,n) (or max???) array of doubles
+	int m; // rows
+	int n; // cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+struct blasfeo_cm_smat
+	{
+	float *mem; // pointer to passed chunk of memory
+	float *pA; // pointer to a m*n array of floats
+	float *dA; // pointer to a min(m,n) (or max???) array of floats
+	int m; // rows
+	int n; // cols
+	int use_dA; // flag to tell if dA can be used
+	int memsize; // size of needed memory
+	};
+
+
+#define BLASFEO_PM_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))])
+#define BLASFEO_PM_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))])
+#define BLASFEO_CM_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m])
+#define BLASFEO_CM_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m])
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_COMMON_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
new file mode 100644
index 000000000..7a9415a59
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h
@@ -0,0 +1,239 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+/*
+ * auxiliary algebra operations header
+ *
+ * include/blasfeo_aux_lib*.h
+ *
+ */
+
+#ifndef BLASFEO_D_AUX_H_
+#define BLASFEO_D_AUX_H_
+
+
+
+#include <stdlib.h>
+
+#include "blasfeo_common.h"
+#include "blasfeo_d_aux_old.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// --- memory size calculations
+//
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_memsize_dmat(int m, int n);
+// returns the memory size (in bytes) needed for the diagonal of a dmat
+size_t blasfeo_memsize_diag_dmat(int m, int n);
+// returns the memory size (in bytes) needed for a dvec
+size_t blasfeo_memsize_dvec(int m);
+
+// --- creation
+//
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory);
+// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated)
+void blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory);
+
+// --- packing
+// pack the column-major matrix A into the matrix struct B
+void blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the lower-triangular column-major matrix A into the matrix struct B
+void blasfeo_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the upper-triangular column-major matrix A into the matrix struct B
+void blasfeo_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// transpose and pack the column-major matrix A into the matrix struct B
+void blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the vector x into the vector structure y
+void blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi);
+// unpack the matrix structure A into the column-major matrix B
+void blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb);
+// transpose and unpack the matrix structure A into the column-major matrix B
+void blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb);
+// pack the vector structure x into the vector y
+void blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y,  int yi);
+
+// --- cast
+//
+//void d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO
+//void d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO
+//void d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO
+
+
+// ge
+// --- insert/extract
+//
+// sA[ai, aj] <= a
+void blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj);
+// <= sA[ai, aj]
+double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- set
+// A <= alpha
+void blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- copy / scale
+// B <= A
+void blasfeo_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// A <= alpha*A
+void blasfeo_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// B <= alpha*A
+void blasfeo_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A, A lower triangular
+void blasfeo_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+void blasfeo_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+void blasfeo_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- sum
+// B <= B + alpha*A
+void blasfeo_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// y <= y + alpha*x
+void blasfeo_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+
+// --- traspositions
+// B <= A'
+void blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A', A lower triangular
+void blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A', A upper triangular
+void blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+
+// dia
+// diag(A) += alpha
+void blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A) <= alpha*x
+void blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A)[idx] <= alpha*x
+void blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// x <= diag(A)
+void blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+// x <= diag(A)[idx]
+void blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi);
+// diag(A) += alpha*x
+void blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A)[idx] += alpha*x
+void blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// diag(A)[idx] = y + alpha*x
+void blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+
+// row
+void blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+void blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+void blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+void blasfeo_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+
+// col
+void blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+void blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+void blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+void blasfeo_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+
+// vec
+// a <= alpha
+void blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi);
+// sx[xi] <= a
+void blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi);
+// <= sx[xi]
+double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi);
+// y <= x
+void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// x <= alpha*x
+void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
+// y <= alpha*x
+void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+
+void blasfeo_dveccl(int m,
+	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
+	struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi);
+
+void blasfeo_dveccl_mask(int m,
+	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
+	struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi,
+	struct blasfeo_dvec *sm, int mi);
+
+void blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei);
+void blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm);
+void blasfeo_dvecnrm_2(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm);
+void blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+void blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+
+
+
+
+
+/*
+* Explicitly panel-major matrix format
+*/
+
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_pm_memsize_dmat(int ps, int m, int n);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_pm_create_dmat(int ps, int m, int n, struct blasfeo_pm_dmat *sA, void *memory);
+// print
+void blasfeo_pm_print_dmat(int m, int n, struct blasfeo_pm_dmat *sA, int ai, int aj);
+
+
+
+/*
+* Explicitly panel-major matrix format
+*/
+
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_cm_memsize_dmat(int m, int n);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_cm_create_dmat(int m, int n, struct blasfeo_pm_dmat *sA, void *memory);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_AUX_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h
new file mode 100644
index 000000000..bee9e986f
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h
@@ -0,0 +1,145 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+/*
+ * auxiliary algebra operation external dependancies header
+ *
+ * include/blasfeo_d_aux_ext_dep.h
+ *
+ * - dynamic memory allocation
+ * - print
+ *
+ */
+
+#ifndef BLASFEO_D_AUX_EXT_DEP_H_
+#define BLASFEO_D_AUX_EXT_DEP_H_
+
+
+
+#include <stdio.h>
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#ifdef EXT_DEP
+
+/* column-major matrices */
+
+// dynamically allocate row*col doubles of memory and set accordingly a pointer to double; set allocated memory to zero
+void d_zeros(double **pA, int row, int col);
+// dynamically allocate row*col doubles of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero
+void d_zeros_align(double **pA, int row, int col);
+// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero
+void d_zeros_align_bytes(double **pA, int size);
+// free the memory allocated by d_zeros
+void d_free(double *pA);
+// free the memory allocated by d_zeros_align or d_zeros_align_bytes
+void d_free_align(double *pA);
+// print a column-major matrix
+void d_print_mat(int m, int n, double *A, int lda);
+// print the transposed of a column-major matrix
+void d_print_tran_mat(int row, int col, double *A, int lda);
+// print to file a column-major matrix
+void d_print_to_file_mat(FILE *file, int row, int col, double *A, int lda);
+// print to file a column-major matrix in exponential format
+void d_print_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda);
+// print to string a column-major matrix
+void d_print_to_string_mat(char **buf_out, int row, int col, double *A, int lda);
+// print to file the transposed of a column-major matrix
+void d_print_tran_to_file_mat(FILE *file, int row, int col, double *A, int lda);
+// print to file the transposed of a column-major matrix in exponential format
+void d_print_tran_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda);
+// print in exponential notation a column-major matrix
+void d_print_exp_mat(int m, int n, double *A, int lda);
+// print in exponential notation the transposed of a column-major matrix
+void d_print_exp_tran_mat(int row, int col, double *A, int lda);
+
+/* strmat and strvec */
+
+// create a strmat for a matrix of size m*n by dynamically allocating memory
+void blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA);
+// create a strvec for a vector of size m by dynamically allocating memory
+void blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa);
+// free the memory allocated by blasfeo_allocate_dmat
+void blasfeo_free_dmat(struct blasfeo_dmat *sA);
+// free the memory allocated by blasfeo_allocate_dvec
+void blasfeo_free_dvec(struct blasfeo_dvec *sa);
+// print a strmat
+void blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print in exponential notation a strmat
+void blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print to file a strmat
+void blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print to file a strmat in exponential format
+void blasfeo_print_to_file_exp_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print to string a strmat
+void blasfeo_print_to_string_dmat(char **buf_out, int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print the transposed of a strmat
+void blasfeo_print_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+// print a strvec
+void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai);
+// print in exponential notation a strvec
+void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai);
+// print to file a strvec
+void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+// print to string a strvec
+void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai);
+// print the transposed of a strvec
+void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+// print in exponential notation the transposed of a strvec
+void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+// print to file the transposed of a strvec
+void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+// print to string the transposed of a strvec
+void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai);
+
+#endif // EXT_DEP
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_AUX_EXT_DEP_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h
new file mode 100644
index 000000000..81b811f03
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h
@@ -0,0 +1,84 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+/*
+ * auxiliary algebra operation external dependancies header
+ *
+ * include/blasfeo_d_aux_ext_dep.h
+ *
+ * - dynamic memory allocation
+ * - print
+ *
+ */
+
+#ifndef BLASFEO_D_AUX_EXT_DEP_REF_H_
+#define BLASFEO_D_AUX_EXT_DEP_REF_H_
+
+
+#include <stdio.h>
+
+#include "blasfeo_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// expose reference BLASFEO for testing
+// see blasfeo_d_aux_exp_dep.h for help
+
+void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_allocate_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA);
+void blasfeo_allocate_dvec_ref(int m, struct blasfeo_dvec_ref *sa);
+void blasfeo_free_dmat_ref(struct blasfeo_dmat_ref *sA);
+void blasfeo_free_dvec_ref(struct blasfeo_dvec_ref *sa);
+void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_print_exp_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_print_to_file_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_print_to_file_exp_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_print_to_string_dmat_ref(char **buf_out, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj);
+void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_AUX_EXT_DEP_REF_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h
new file mode 100644
index 000000000..3a1847a6a
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h
@@ -0,0 +1,75 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+/*
+ * ----------- TOMOVE
+ *
+ * expecting column major matrices
+ *
+ */
+
+#include "blasfeo_common.h"
+
+
+void dtrcp_l_lib(int m, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb);
+void dgead_lib(int m, int n, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb);
+// TODO remove ???
+void ddiain_sqrt_lib(int kmax, double *x, int offset, double *pD, int sdd);
+// TODO ddiaad1
+void ddiareg_lib(int kmax, double reg, int offset, double *pD, int sdd);
+
+
+void dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+void dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+void dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+void ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x);
+void ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd);
+void ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd);
+void ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x);
+void ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd);
+void ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd);
+void drowin_lib(int kmax, double alpha, double *x, double *pD);
+void drowex_lib(int kmax, double alpha, double *pD, double *x);
+void drowad_lib(int kmax, double alpha, double *x, double *pD);
+void drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD);
+void drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD);
+void drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD);
+void dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd);
+void dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd);
+void dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd);
+void dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd);
+void dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+void dvecin_libsp(int kmax, int *idx, double *x, double *y);
+void dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y);
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
new file mode 100644
index 000000000..448234044
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h
@@ -0,0 +1,206 @@
+
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_AUX_REF_H_
+#define BLASFEO_D_AUX_REF_H_
+
+
+
+#include <stdlib.h>
+
+#include "blasfeo_common.h"
+#include "blasfeo_d_aux_old.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// --- memory calculations
+//
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_ref_memsize_dmat(int m, int n);
+// returns the memory size (in bytes) needed for the diagonal of a dmat
+size_t blasfeo_ref_memsize_diag_dmat(int m, int n);
+// returns the memory size (in bytes) needed for a dvec
+size_t blasfeo_ref_memsize_dvec(int m);
+
+// --- creation
+//
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_ref_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory);
+// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated)
+void blasfeo_ref_create_dvec(int m, struct blasfeo_dvec *sA, void *memory);
+
+// --- packing
+// pack the column-major matrix A into the matrix struct B
+void blasfeo_ref_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the lower-triangular column-major matrix A into the matrix struct B
+void blasfeo_ref_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the upper-triangular column-major matrix A into the matrix struct B
+void blasfeo_ref_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// transpose and pack the column-major matrix A into the matrix struct B
+void blasfeo_ref_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
+// pack the vector x into the vector structure y
+void blasfeo_ref_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi);
+// unpack the matrix structure A into the column-major matrix B
+void blasfeo_ref_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb);
+// transpose and unpack the matrix structure A into the column-major matrix B
+void blasfeo_ref_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb);
+// pack the vector structure x into the vector y
+void blasfeo_ref_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y,  int yi);
+
+// --- cast
+//
+void ref_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO
+void ref_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO
+void ref_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO
+
+
+// ge
+// --- insert/extract
+//
+// sA[ai, aj] <= a
+void blasfeo_ref_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj);
+// <= sA[ai, aj]
+double blasfeo_ref_dgeex1(struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- set
+// A <= alpha
+void blasfeo_ref_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- copy / scale
+// B <= A
+void blasfeo_ref_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// A <= alpha*A
+void blasfeo_ref_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// B <= alpha*A
+void blasfeo_ref_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A, A lower triangular
+void blasfeo_ref_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+void blasfeo_ref_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+void blasfeo_ref_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+
+// --- sum
+// B <= B + alpha*A
+void blasfeo_ref_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int yi, int cj);
+// y <= y + alpha*x
+void blasfeo_ref_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+
+// --- traspositions
+// B <= A'
+void blasfeo_ref_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A', A lower triangular
+void blasfeo_ref_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+// B <= A', A upper triangular
+void blasfeo_ref_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj);
+
+// dia
+// diag(A) += alpha
+void blasfeo_ref_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A) <= alpha*x
+void blasfeo_ref_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A)[idx] <= alpha*x
+void blasfeo_ref_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// x <= diag(A)
+void blasfeo_ref_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+// x <= diag(A)[idx]
+void blasfeo_ref_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi);
+// diag(A) += alpha*x
+void blasfeo_ref_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// diag(A)[idx] += alpha*x
+void blasfeo_ref_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// diag(A)[idx] = y + alpha*x
+void blasfeo_ref_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+
+// row
+void blasfeo_ref_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_ref_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+void blasfeo_ref_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_ref_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_ref_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+void blasfeo_ref_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+void blasfeo_ref_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+
+// col
+void blasfeo_ref_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+void blasfeo_ref_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_ref_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_ref_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+void blasfeo_ref_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+void blasfeo_ref_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+void blasfeo_ref_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+
+// vec
+// a <= alpha
+void blasfeo_ref_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi);
+// sx[xi] <= a
+void blasfeo_ref_dvecin1(double a, struct blasfeo_dvec *sx, int xi);
+// <= sx[xi]
+double blasfeo_ref_dvecex1(struct blasfeo_dvec *sx, int xi);
+// y <= x
+void blasfeo_ref_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// x <= alpha*x
+void blasfeo_ref_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi);
+// y <= alpha*x
+void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+
+void blasfeo_ref_dveccl(int m,
+	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
+	struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi);
+
+void blasfeo_ref_dveccl_mask(int m,
+	struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi,
+	struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi,
+	struct blasfeo_dvec *sm, int mi);
+
+void blasfeo_ref_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei);
+void blasfeo_ref_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm);
+void blasfeo_ref_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+void blasfeo_ref_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_AUX_REF_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h
new file mode 100644
index 000000000..1c61635f3
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h
@@ -0,0 +1,226 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+/*
+ * auxiliary algebra operations header
+ *
+ * include/blasfeo_aux_lib*.h
+ *
+ */
+
+#ifndef BLASFEO_D_AUX_TEST_H_
+#define BLASFEO_D_AUX_TEST_H_
+
+#include "blasfeo_common.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// --- memory calculations
+int test_blasfeo_memsize_dmat(int m, int n);
+int test_blasfeo_memsize_diag_dmat(int m, int n);
+int test_blasfeo_memsize_dvec(int m);
+
+// --- creation
+void test_blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory);
+void test_blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory);
+
+// --- conversion
+void test_blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj);
+void test_blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sa, int ai);
+void test_blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj);
+void test_blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda);
+void test_blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sa, int ai, double *x, int xi);
+void test_blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda);
+
+// --- cast
+void test_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA);
+void test_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA);
+void test_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sa);
+
+// ------ copy / scale
+
+// B <= A
+void test_blasfeo_dgecp(int m, int n,
+					struct blasfeo_dmat *sA, int ai, int aj,
+					struct blasfeo_dmat *sB, int bi, int bj);
+
+// A <= alpha*A
+void test_blasfeo_dgesc(int m, int n,
+					double alpha,
+					struct blasfeo_dmat *sA, int ai, int aj);
+
+// B <= alpha*A
+void test_blasfeo_dgecpsc(int m, int n,
+					double alpha,
+					struct blasfeo_dmat *sA, int ai, int aj,
+					struct blasfeo_dmat *sB, int bi, int bj);
+
+// // --- insert/extract
+// //
+// // <= sA[ai, aj]
+// void test_blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj);
+// // <= sA[ai, aj]
+// double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj);
+// // sx[xi] <= a
+// void test_blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi);
+// // <= sx[xi]
+// double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi);
+// // A <= alpha
+
+// // --- set
+// void test_blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// // a <= alpha
+// void test_blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi);
+// // B <= A
+
+
+// // --- vector
+// // y <= x
+// void test_blasfeo_dveccp(int m, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci);
+// // x <= alpha*x
+// void test_blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sa, int ai);
+// // TODO
+// // x <= alpha*x
+// void test_blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci);
+
+
+// // B <= A, A lower triangular
+// void test_blasfeo_dtrcp_l(int m,
+//                     struct blasfeo_dmat *sA, int ai, int aj,
+//                     struct blasfeo_dmat *sB, int bi, int bj);
+
+// void test_blasfeo_dtrcpsc_l(int m, double alpha,
+//                     struct blasfeo_dmat *sA, int ai, int aj,
+//                     struct blasfeo_dmat *sB, int bi, int bj);
+
+// void test_blasfeo_dtrsc_l(int m, double alpha,
+//                     struct blasfeo_dmat *sA, int ai, int aj);
+
+
+// // B <= B + alpha*A
+// void test_blasfeo_dgead(int m, int n, double alpha,
+//                     struct blasfeo_dmat *sA, int ai, int aj,
+//                     struct blasfeo_dmat *sC, int ci, int cj);
+
+// // y <= y + alpha*x
+// void test_blasfeo_dvecad(int m, double alpha,
+//                     struct blasfeo_dvec *sa, int ai,
+//                     struct blasfeo_dvec *sc, int ci);
+
+// // --- traspositions
+// void test_dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+// void test_blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+// void test_dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+// void test_blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+// void test_dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+// void test_blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+// void test_blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x);
+// void test_ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd);
+// void test_ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd);
+// void test_blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// void test_ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x);
+// void test_blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+// void test_blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi);
+// void test_blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd);
+// void test_blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// void test_ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd);
+// void test_blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// void test_drowin_lib(int kmax, double alpha, double *x, double *pD);
+// void test_blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_drowex_lib(int kmax, double alpha, double *pD, double *x);
+// void test_blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+// void test_drowad_lib(int kmax, double alpha, double *x, double *pD);
+// void test_blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD);
+// void test_drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD);
+// void test_blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj);
+// void test_drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD);
+// void test_drowsw_lib(int kmax, double *pA, double *pC);
+// void test_blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+// void test_blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+// void test_blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi);
+// void test_dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd);
+// void test_blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj);
+// void test_dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd);
+// void test_dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd);
+// void test_dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd);
+// void test_dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc);
+// void test_blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj);
+// void test_blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA);
+// void test_dvecin_libsp(int kmax, int *idx, double *x, double *y);
+// void test_dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y);
+// void test_blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+// void test_blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi);
+// void test_blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi);
+// void test_blasfeo_dveccl(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi);
+// void test_blasfeo_dveccl_mask(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, struct blasfeo_dvec *sm, int mi);
+// void test_blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei);
+// void test_blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm);
+// void test_blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+// void test_blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi);
+
+// ext_dep
+
+void test_blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA);
+void test_blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa);
+
+void test_blasfeo_free_dmat(struct blasfeo_dmat *sA);
+void test_blasfeo_free_dvec(struct blasfeo_dvec *sa);
+
+void test_blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+void test_blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void test_blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+
+void test_blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+void test_blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+void test_blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai);
+
+void test_blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj);
+void test_blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai);
+void test_blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_AUX_TEST_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h
new file mode 100644
index 000000000..ea8d00673
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h
@@ -0,0 +1,46 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_BLAS_H_
+#define BLASFEO_D_BLAS_H_
+
+
+
+#include "blasfeo_d_blasfeo_api.h"
+#include "blasfeo_d_blas_api.h"
+
+
+
+#endif  // BLASFEO_D_BLAS_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h
new file mode 100644
index 000000000..64dd43f67
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h
@@ -0,0 +1,161 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef BLASFEO_D_BLAS_API_H_
+#define BLASFEO_D_BLAS_API_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#ifdef BLAS_API
+
+
+
+#ifdef FORTRAN_BLAS_API
+
+
+
+// BLAS 1
+//
+void daxpy_(int *n, double *alpha, double *x, int *incx, double *y, int *incy);
+//
+void dcopy_(int *n, double *x, int *incx, double *y, int *incy);
+//
+double ddot_(int *n, double *x, int *incx, double *y, int *incy);
+
+// BLAS 3
+//
+void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc);
+//
+void dsyrk_(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc);
+//
+void dtrmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+//
+void dtrsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+
+
+
+// LAPACK
+//
+void dgesv_(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info);
+//
+void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info);
+//
+void dgetrf_np_(int *m, int *n, double *A, int *lda, int *info);
+//
+void dgetrs_(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info);
+//
+void dlaswp_(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx);
+//
+void dposv_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+//
+void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info);
+//
+void dpotrs_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+//
+void dtrtrs_(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+
+
+
+#else // BLASFEO_API
+
+
+
+// BLAS 1
+//
+void blas_daxpy(int *n, double *alpha, double *x, int *incx, double *y, int *incy);
+//
+double blas_ddot(int *n, double *x, int *incx, double *y, int *incy);
+//
+void blas_dcopy(int *n, double *x, int *incx, double *y, int *incy);
+
+// BLAS 3
+//
+void blas_dgemm(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc);
+//
+void blas_dsyrk(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc);
+//
+void blas_dtrmm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+//
+void blas_dtrsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+
+
+
+// LAPACK
+//
+void blas_dgesv(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info);
+//
+void blas_dgetrf(int *m, int *n, double *A, int *lda, int *ipiv, int *info);
+//
+void blas_dgetrf_np(int *m, int *n, double *A, int *lda, int *info);
+//
+void blas_dgetrs(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info);
+//
+void blas_dlaswp(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx);
+//
+void blas_dposv(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+//
+void blas_dpotrf(char *uplo, int *m, double *A, int *lda, int *info);
+//
+void blas_dpotrs(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+//
+void blas_dtrtrs(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info);
+
+
+
+#endif // BLASFEO_API
+
+
+
+#endif // BLAS_API
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_BLAS_API_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h
new file mode 100644
index 000000000..f27264e13
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h
@@ -0,0 +1,342 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_BLASFEO_API_H_
+#define BLASFEO_D_BLASFEO_API_H_
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+// level 1 BLAS
+//
+
+// z = y + alpha*x
+// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n]
+// NB: Different arguments semantic compare to equivalent standard BLAS routine
+void blasfeo_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = beta*y + alpha*x
+void blasfeo_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = x .* y
+void blasfeo_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z += x .* y
+void blasfeo_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = x .* y, return sum(z) = x^T * y
+double blasfeo_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// return x^T * y
+double blasfeo_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// construct givens plane rotation
+void blasfeo_drotg(double a, double b, double *c, double *s);
+// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai
+void blasfeo_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s);
+// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj
+void blasfeo_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s);
+
+
+
+//
+// level 2 BLAS
+//
+
+// dense
+
+// z <= beta * y + alpha * A * x
+void blasfeo_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z <= beta * y + alpha * A^T * x
+void blasfeo_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(n)
+void blasfeo_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A^T ) * x, A (m)x(n)
+void blasfeo_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit
+void blasfeo_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit
+void blasfeo_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, not_unit
+void blasfeo_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, unit
+void blasfeo_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A^T ) * x, A (m)x(m) upper, not_transposed, not_unit
+void blasfeo_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A^T ) * x, A (m)x(m) upper, transposed, not_unit
+void blasfeo_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= beta * y + alpha * A * x ; A upper triangular
+void blasfeo_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A^T * x ; A upper triangular
+void blasfeo_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A * x ; A lower triangular
+void blasfeo_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A^T * x ; A lower triangular
+void blasfeo_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A * x ; A lower triangular, unit diagonal
+void blasfeo_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A^T * x ; A lower triangular, unit diagonal
+void blasfeo_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z_n <= beta_n * y_n + alpha_n * A  * x_n
+// z_t <= beta_t * y_t + alpha_t * A^T * x_t
+void blasfeo_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t);
+// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
+void blasfeo_dsymv_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+void blasfeo_dsymv_l_mn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+
+// diagonal
+
+// z <= beta * y + alpha * A * x, A diagonal
+void blasfeo_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+
+
+
+//
+// level 3 BLAS
+//
+
+// dense
+
+// D <= beta * C + alpha * A * B
+void blasfeo_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T
+void blasfeo_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B^T
+void blasfeo_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D lower triangular
+void blasfeo_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) )
+void blasfeo_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#endif
+// D <= beta * C + alpha * A^T * B ; C, D lower triangular
+void blasfeo_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) )
+void blasfeo_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#endif
+// D <= beta * C + alpha * A * B^T ; C, D upper triangular
+void blasfeo_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) )
+void blasfeo_dsyrk3_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#endif
+// D <= beta * C + alpha * A^T * B ; C, D upper triangular
+void blasfeo_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) )
+void blasfeo_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+#endif
+// D <= alpha * A * B ; A lower triangular
+void blasfeo_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A lower triangular
+void blasfeo_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A lower triangular
+void blasfeo_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A lower triangular
+void blasfeo_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A upper triangular
+void blasfeo_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A upper triangular
+void blasfeo_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A upper triangular
+void blasfeo_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A upper triangular
+void blasfeo_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A lower triangular
+void blasfeo_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A lower triangular
+void blasfeo_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A upper triangular
+void blasfeo_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A upper triangular
+void blasfeo_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A upper triangular
+void blasfeo_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A upper triangular
+void blasfeo_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal
+// D <= alpha * A^{-1} * B , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal
+void blasfeo_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal
+void blasfeo_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular with unit diagonal
+void blasfeo_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular with unit diagonal
+void blasfeo_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal
+void blasfeo_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal
+void blasfeo_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal
+void blasfeo_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal
+void blasfeo_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+
+// diagonal
+
+// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec)
+void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd);
+void blasfeo_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec)
+void blasfeo_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+
+
+
+//
+// LAPACK
+//
+
+// D <= chol( C ) ; C, D lower triangular
+void blasfeo_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= chol( C ) ; C, D upper triangular
+void blasfeo_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= chol( C + A * B' ) ; C, D lower triangular
+// D <= chol( C + A * B^T ) ; C, D lower triangular
+void blasfeo_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= lu( C ) ; no pivoting
+void blasfeo_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= lu( C ) ; row pivoting
+void blasfeo_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv);
+// D <= qr( C )
+int blasfeo_dgeqrf_worksize(int m, int n); // in bytes
+void blasfeo_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// D <= Q factor, where C is the output of the LQ factorization
+int blasfeo_dorglq_worksize(int m, int n, int k); // in bytes
+void blasfeo_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// D <= lq( C )
+void blasfeo_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+int blasfeo_dgelqf_worksize(int m, int n); // in bytes
+// D <= lq( C ), positive diagonal elements
+void blasfeo_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work);
+// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with:
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work);
+
+
+
+//
+// BLAS API helper functions
+//
+
+#if ( defined(BLAS_API) & defined(MF_PANELMAJ) )
+// BLAS 3
+void blasfeo_cm_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk3_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj);
+// LAPACK
+void blasfeo_cm_dpotrf_l(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dpotrf_u(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj);
+void blasfeo_cm_dgetrf_rp(int m, int n, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj, int *ipiv);
+#endif
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_BLASFEO_API_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h
new file mode 100644
index 000000000..141a1f0f0
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h
@@ -0,0 +1,147 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_BLASFEO_API_REF_H_
+#define BLASFEO_D_BLASFEO_API_REF_H_
+
+#include "blasfeo_common.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// expose reference BLASFEO for testing
+
+// --- level 1
+
+void blasfeo_daxpy_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_daxpby_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dvecmul_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dvecmulacc_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+double blasfeo_dvecmuldot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+double blasfeo_ddot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi);
+void blasfeo_drotg_ref(double a, double b, double *c, double *s);
+void blasfeo_dcolrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj0, int aj1, double c, double s);
+void blasfeo_drowrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai0, int ai1, int aj, double c, double s);
+
+
+// --- level 2
+
+// dense
+void blasfeo_dgemv_n_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dgemv_t_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_lnn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_ltn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_lnn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_lnu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_ltn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_ltu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrsv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_lnn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_ltn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_lnu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dtrmv_ltu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi);
+void blasfeo_dgemv_nt_ref(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx_n, int xi_n, struct blasfeo_dvec_ref *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec_ref *sy_n, int yi_n, struct blasfeo_dvec_ref *sy_t, int yi_t, struct blasfeo_dvec_ref *sz_n, int zi_n, struct blasfeo_dvec_ref *sz_t, int zi_t);
+void blasfeo_dsymv_l_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+
+// diagonal
+void blasfeo_dgemv_d_ref(int m, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi);
+
+
+// --- level 3
+
+// dense
+void blasfeo_dgemm_nn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgemm_nt_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgemm_tn_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgemm_tt_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+
+void blasfeo_dsyrk_ln_mn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_ln_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_lt_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_un_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_ut_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+
+void blasfeo_dtrmm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrmm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+
+void blasfeo_dtrsm_lunu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_lunn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_lutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_lutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_llnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_llnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_lltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_lltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_runu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_runn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rlnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dtrsm_rltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj);
+
+// diagonal
+void dgemm_diag_left_lib_ref(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd);
+void blasfeo_dgemm_dn_ref(int m, int n, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgemm_nd_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sB, int bi, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+
+// --- lapack
+
+void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv);
+void blasfeo_dpotrf_l_ref(int m, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dpotrf_l_mn_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dsyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj);
+void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv);
+void blasfeo_dgeqrf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work);
+void blasfeo_dgelqf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work);
+void blasfeo_dgelqf_pd_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work);
+void blasfeo_dgelqf_pd_la_ref(int m, int n1, struct blasfeo_dmat_ref *sL, int li, int lj, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work);
+void blasfeo_dgelqf_pd_lla_ref(int m, int n1, struct blasfeo_dmat_ref *sL0, int l0i, int l0j, struct blasfeo_dmat_ref *sL1, int l1i, int l1j, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_BLASFEO_API_REF_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h
new file mode 100644
index 000000000..eaac36581
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h
@@ -0,0 +1,270 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_BLASFEO_REF_API_H_
+#define BLASFEO_D_BLASFEO_REF_API_H_
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+// level 1 BLAS
+//
+
+// z = y + alpha*x
+// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n]
+// NB: Different arguments semantic compare to equivalent standard BLAS routine
+void blasfeo_ref_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = beta*y + alpha*x
+void blasfeo_ref_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = x .* y
+void blasfeo_ref_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z += x .* y
+void blasfeo_ref_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z = x .* y, return sum(z) = x^T * y
+double blasfeo_ref_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// return x^T * y
+double blasfeo_ref_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi);
+// construct givens plane rotation
+void blasfeo_ref_drotg(double a, double b, double *c, double *s);
+// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai
+void blasfeo_ref_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s);
+// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj
+void blasfeo_ref_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s);
+
+
+
+//
+// level 2 BLAS
+//
+
+// dense
+
+// z <= beta * y + alpha * A * x
+void blasfeo_ref_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z <= beta * y + alpha * A' * x
+void blasfeo_ref_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(n)
+void blasfeo_ref_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(n)
+void blasfeo_ref_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit
+void blasfeo_ref_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit
+void blasfeo_ref_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit
+void blasfeo_ref_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit
+void blasfeo_ref_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit
+void blasfeo_ref_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
+void blasfeo_ref_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= beta * y + alpha * A * x ; A upper triangular
+void blasfeo_ref_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A' * x ; A upper triangular
+void blasfeo_ref_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A * x ; A lower triangular
+void blasfeo_ref_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A' * x ; A lower triangular
+void blasfeo_ref_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A * x ; A lower triangular, unit diagonal
+void blasfeo_ref_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z <= A' * x ; A lower triangular, unit diagonal
+void blasfeo_ref_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
+// z_n <= beta_n * y_n + alpha_n * A  * x_n
+// z_t <= beta_t * y_t + alpha_t * A' * x_t
+void blasfeo_ref_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t);
+// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
+void blasfeo_ref_dsymv_l(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+
+// diagonal
+
+// z <= beta * y + alpha * A * x, A diagonal
+void blasfeo_ref_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
+
+
+
+//
+// level 3 BLAS
+//
+
+// dense
+
+// D <= beta * C + alpha * A * B
+void blasfeo_ref_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T
+void blasfeo_ref_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_ref_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B^T
+void blasfeo_ref_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D lower triangular
+void blasfeo_ref_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_ref_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D lower triangular
+void blasfeo_ref_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D upper triangular
+void blasfeo_ref_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D upper triangular
+void blasfeo_ref_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A lower triangular
+void blasfeo_ref_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A lower triangular
+void blasfeo_ref_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A lower triangular
+void blasfeo_ref_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A lower triangular
+void blasfeo_ref_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A upper triangular
+void blasfeo_ref_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B ; A upper triangular
+void blasfeo_ref_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A upper triangular
+void blasfeo_ref_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^T * B ; A upper triangular
+void blasfeo_ref_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_ref_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_ref_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A lower triangular
+void blasfeo_ref_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A lower triangular
+void blasfeo_ref_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A upper triangular
+void blasfeo_ref_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A ; A upper triangular
+void blasfeo_ref_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A upper triangular
+void blasfeo_ref_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^T ; A upper triangular
+void blasfeo_ref_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_ref_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal
+void blasfeo_ref_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_ref_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal
+void blasfeo_ref_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal
+void blasfeo_ref_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal
+void blasfeo_ref_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal
+void blasfeo_ref_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal
+void blasfeo_ref_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal
+void blasfeo_ref_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal
+void blasfeo_ref_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj);
+
+// diagonal
+
+// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec)
+void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd);
+void blasfeo_ref_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec)
+void blasfeo_ref_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+
+
+
+//
+// LAPACK
+//
+
+// D <= chol( C ) ; C, D lower triangular
+void blasfeo_ref_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_ref_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= chol( C ) ; C, D upper triangular
+void blasfeo_ref_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= chol( C + A * B' ) ; C, D lower triangular
+void blasfeo_ref_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+void blasfeo_ref_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= lu( C ) ; no pivoting
+void blasfeo_ref_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj);
+// D <= lu( C ) ; row pivoting
+void blasfeo_ref_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv);
+// D <= qr( C )
+int blasfeo_ref_dgeqrf_worksize(int m, int n); // in bytes
+void blasfeo_ref_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// D <= Q factor, where C is the output of the LQ factorization
+int blasfeo_ref_dorglq_worksize(int m, int n, int k); // in bytes
+void blasfeo_ref_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// D <= lq( C )
+void blasfeo_ref_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+int blasfeo_ref_dgelqf_worksize(int m, int n); // in bytes
+// D <= lq( C ), positive diagonal elements
+void blasfeo_ref_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work);
+// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_ref_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work);
+// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with:
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_ref_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_BLASFEO_REF_API_H_
+
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h b/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h
new file mode 100644
index 000000000..bf110c2bf
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h
@@ -0,0 +1,1048 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_D_KERNEL_H_
+#define BLASFEO_D_KERNEL_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// utils
+void blasfeo_align_2MB(void *ptr, void **ptr_align);
+void blasfeo_align_4096_byte(void *ptr, void **ptr_align);
+void blasfeo_align_64_byte(void *ptr, void **ptr_align);
+
+
+
+// level 2 BLAS
+// 12
+void kernel_dgemv_n_12_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z);
+void kernel_dgemv_t_12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z);
+// 8
+void kernel_dgemv_n_8_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z);
+void kernel_dgemv_t_8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z);
+void kernel_dtrmv_un_8_lib4(int k, double *A, int sda, double *x, double *z);
+// 4
+void kernel_dgemv_n_4_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z);
+void kernel_dgemv_n_4_vs_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k1);
+void kernel_dgemv_n_4_gen_lib4(int kmax, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k0, int k1);
+void kernel_dgemv_t_4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z);
+void kernel_dgemv_t_4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z, int k1);
+void kernel_dtrsv_ln_inv_4_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_ln_inv_4_vs_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z, int km, int kn);
+void kernel_dtrsv_lt_inv_4_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_lt_inv_3_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_lt_inv_2_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_lt_inv_1_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_lt_one_4_lib4(int k, double *A, int sda, double *x, double *y, double *z);
+void kernel_dtrsv_lt_one_3_lib4(int k, double *A, int sda, double *x, double *y, double *z);
+void kernel_dtrsv_lt_one_2_lib4(int k, double *A, int sda, double *x, double *y, double *z);
+void kernel_dtrsv_lt_one_1_lib4(int k, double *A, int sda, double *x, double *y, double *z);
+void kernel_dtrsv_ln_one_4_vs_lib4(int kmax, double *A, double *x, double *y, double *z, int km, int kn);
+void kernel_dtrsv_ln_one_4_lib4(int kmax, double *A, double *x, double *y, double *z);
+void kernel_dtrsv_un_inv_4_lib4(int kmax, double *A, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_ut_inv_4_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z);
+void kernel_dtrsv_ut_inv_4_vs_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z, int m1, int n1);
+void kernel_dtrmv_un_4_lib4(int k, double *A, double *x, double *z);
+void kernel_dtrmv_ut_4_lib4(int k, double *A, int sda, double *x, double *z);
+void kernel_dtrmv_ut_4_vs_lib4(int k, double *A, int sda, double *x, double *z, int km);
+void kernel_dgemv_nt_6_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t);
+void kernel_dgemv_nt_4_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t);
+void kernel_dgemv_nt_4_vs_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t, int km);
+void kernel_dsymv_l_4_lib4(int kmax, double *alpha, double *A, int sda, double *x, double *z);
+void kernel_dsymv_l_4_gen_lib4(int kmax, double *alpha, int offA, double *A, int sda, double *x, double *z, int km);
+
+
+
+// level 3 BLAS
+// 12x4
+void kernel_dgemm_nt_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nt_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); //
+void kernel_dgemm_nt_12x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1);
+void kernel_dgemm_nn_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); //
+void kernel_dgemm_nn_12x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dgemm_tt_12x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_tt_12x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dsyrk_nn_u_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dsyrk_nn_u_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dsyrk_nt_l_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dsyrk_nt_l_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); //
+void kernel_dtrmm_nt_ru_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); //
+void kernel_dtrmm_nt_ru_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); //
+void kernel_dtrmm_nn_rl_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd);
+void kernel_dtrmm_nn_rl_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn);
+void kernel_dtrsm_nt_rl_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_rl_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E);
+void kernel_dtrsm_nt_rl_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn);
+void kernel_dtrsm_nt_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_ru_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E);
+void kernel_dtrsm_nt_ru_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn);
+void kernel_dtrsm_nn_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E);
+void kernel_dtrsm_nn_ll_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_one_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde);
+void kernel_dtrsm_nn_ll_one_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn);
+void kernel_dtrsm_nn_lu_inv_12x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E);
+void kernel_dtrsm_nn_lu_inv_12x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+// 4x12
+void kernel_dgemm_nt_4x12_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nt_4x12_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dgemm_nn_4x12_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_4x12_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_tt_4x12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_tt_4x12_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_tt_4x12_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dtrsm_nt_rl_inv_4x12_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_4x12_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn);
+// 8x8
+void kernel_dgemm_nt_8x8l_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11]
+void kernel_dgemm_nt_8x8u_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11]
+void kernel_dgemm_nt_8x8l_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11]
+void kernel_dgemm_nt_8x8u_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11]
+void kernel_dsyrk_nn_u_8x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dsyrk_nn_u_8x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dsyrk_nt_l_8x8_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [L00 *; A10 L11]
+void kernel_dsyrk_nt_l_8x8_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [L00 *; A10 L11]
+void kernel_dtrsm_nt_rl_inv_8x8l_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_8x8l_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_rl_inv_8x8u_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_8x8u_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn);
+// 8x4
+void kernel_dgemm_nt_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nt_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); //
+void kernel_dgemm_nt_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1);
+void kernel_dgemm_nn_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dgemm_tt_8x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_tt_8x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dsyrk_nn_u_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dsyrk_nn_u_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dsyrk_nt_l_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dsyrk_nt_l_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); //
+void kernel_dsyrk_nt_l_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1);
+void kernel_dtrmm_nt_ru_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); //
+void kernel_dtrmm_nt_ru_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); //
+void kernel_dtrmm_nn_rl_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd);
+void kernel_dtrmm_nn_rl_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn);
+void kernel_dtrmm_nn_rl_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_rl_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E);
+void kernel_dtrsm_nt_rl_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn);
+void kernel_dtrsm_nt_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_ru_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E);
+void kernel_dtrsm_nt_ru_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn);
+void kernel_dtrsm_nn_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E);
+void kernel_dtrsm_nn_ll_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_one_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde);
+void kernel_dtrsm_nn_ll_one_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn);
+void kernel_dtrsm_nn_lu_inv_8x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E);
+void kernel_dtrsm_nn_lu_inv_8x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+// 4x8
+void kernel_dgemm_nt_4x8_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nt_4x8_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dgemm_nn_4x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_4x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_tt_4x8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_tt_4x8_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_tt_4x8_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dtrsm_nt_rl_inv_4x8_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_4x8_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn);
+// 8x2
+void kernel_dgemm_nn_8x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_8x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+// 2x8
+void kernel_dgemm_nn_2x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_2x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+// 10xX
+void kernel_dgemm_nn_10x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_10x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_10x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_10x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+// 6xX
+void kernel_dgemm_nn_8x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_8x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_6x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_6x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_6x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_6x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_6x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_6x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+void kernel_dgemm_nn_6x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
+void kernel_dgemm_nn_6x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
+// 4x4
+void kernel_dgemm_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dgemm_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dgemm_nt_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_dgemm_nn_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_nn_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dgemm_tt_4x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D); //
+void kernel_dgemm_tt_4x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_tt_4x4_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_dsyrk_nn_u_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dsyrk_nn_u_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dsyrk_nt_l_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dsyrk_nt_l_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dsyrk_nt_l_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_dsyrk_nt_u_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dsyrk_nt_u_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dsyrk_nt_u_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_dtrmm_nt_ru_4x4_lib4(int k, double *alpha, double *A, double *B, double *D); //
+void kernel_dtrmm_nt_ru_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *D, int km, int kn); //
+void kernel_dtrmm_nn_rl_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D);
+void kernel_dtrmm_nn_rl_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1);
+void kernel_dtrmm_nn_rl_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_dtrsm_nt_rl_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_rl_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E);
+void kernel_dtrsm_nt_rl_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn);
+void kernel_dtrsm_nt_ru_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nt_ru_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E);
+void kernel_dtrsm_nt_ru_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn);
+void kernel_dtrsm_nn_ru_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nn_ll_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_ll_one_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E);
+void kernel_dtrsm_nn_ll_one_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, int km, int kn);
+void kernel_dtrsm_nn_lu_inv_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nn_lu_inv_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dtrsm_nn_lu_one_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E);
+void kernel_dtrsm_nn_lu_one_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, int km, int kn);
+// 4x2
+void kernel_dgemm_nn_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+void kernel_dgemm_nt_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D);
+void kernel_dgemm_nt_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1);
+void kernel_dsyrk_nt_l_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dsyrk_nt_l_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+void kernel_dtrmm_nn_rl_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D);
+void kernel_dtrmm_nn_rl_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_4x2_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dtrsm_nt_rl_inv_4x2_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+// 2x4
+void kernel_dgemm_nn_2x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dgemm_nn_2x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
+// 2x2
+void kernel_dgemm_nn_2x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); //
+void kernel_dsyrk_nt_l_2x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dsyrk_nt_l_2x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+// diag
+void kernel_dgemm_diag_right_4_a0_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *D, int sdd);
+void kernel_dgemm_diag_right_4_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd);
+void kernel_dgemm_diag_right_3_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd);
+void kernel_dgemm_diag_right_2_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd);
+void kernel_dgemm_diag_right_1_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd);
+void kernel_dgemm_diag_left_4_a0_lib4(int kmax, double *alpha, double *A, double *B, double *D);
+void kernel_dgemm_diag_left_4_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D);
+void kernel_dgemm_diag_left_3_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D);
+void kernel_dgemm_diag_left_2_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D);
+void kernel_dgemm_diag_left_1_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D);
+// low rank update
+void kernel_dger4_sub_12r_lib4(int k, double *A, int sda, double *B, double *C, int sdc);
+void kernel_dger4_sub_12r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km);
+void kernel_dger4_sub_8r_lib4(int k, double *A, int sda, double *B, double *C, int sdc);
+void kernel_dger4_sub_8r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km);
+void kernel_dger4_sub_4r_lib4(int n, double *A, double *B, double *C);
+void kernel_dger4_sub_4r_vs_lib4(int n, double *A, double *B, double *C, int km);
+
+
+
+// LAPACK
+// 12x4
+void kernel_dpotrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dpotrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nn_l_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nn_l_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nn_m_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nn_m_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nn_r_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nn_r_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_m_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nt_m_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_r_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nt_r_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+// 8x8
+void kernel_dpotrf_nt_l_8x8_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dpotrf_nt_l_8x8_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+// 8x4
+void kernel_dpotrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dpotrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nn_l_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nn_l_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nn_r_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nn_r_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_r_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dgetrf_nt_r_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+// 4x4
+void kernel_dpotrf_nt_l_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D);
+void kernel_dpotrf_nt_l_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn);
+#if defined(TARGET_X64_INTEL_SANDY_BRIDGE)
+void kernel_dlauum_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
+void kernel_dlauum_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); //
+#endif
+void kernel_dgetrf_nn_4x4_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D);
+void kernel_dgetrf_nn_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D, int km, int kn);
+void kernel_dgetrf_nt_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D);
+void kernel_dgetrf_nt_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn);
+void kernel_dgeqrf_4_lib4(int m, double *pD, int sdd, double *dD);
+void kernel_dgeqrf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD);
+void kernel_dlarf_4_lib4(int m, int n, double *pV, int sdv, double *tau, double *pC, int sdc); // rank-4 reflector
+void kernel_dlarf_t_4_lib4(int m, int n, double *pD, int sdd, double *pVt, double *dD, double *pC0, int sdc, double *pW);
+void kernel_dgelqf_4_lib4(int n, double *pD, double *dD);
+void kernel_dgelqf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD);
+void kernel_dlarft_4_lib4(int kmax, double *pD, double *dD, double *pT);
+void kernel_dlarft_3_lib4(int kmax, double *pD, double *dD, double *pT);
+void kernel_dlarft_2_lib4(int kmax, double *pD, double *dD, double *pT);
+void kernel_dlarft_1_lib4(int kmax, double *pD, double *dD, double *pT);
+void kernel_dgelqf_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT);
+void kernel_dgelqf_dlarft4_12_lib4(int n, double *pD, int sdd, double *dD, double *pT);
+void kernel_dgelqf_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT);
+void kernel_dgelqf_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT);
+void kernel_dlarfb12_rn_12_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK);
+void kernel_dlarfb12_rn_4_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK, int km);
+void kernel_dlarfb4_rn_12_lib4(int kmax, double *pV, double *pT, double *pD, int sdd);
+void kernel_dlarfb4_rn_8_lib4(int kmax, double *pV, double *pT, double *pD, int sdd);
+void kernel_dlarfb4_rn_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb3_rn_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb2_rn_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb1_rn_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb4_rn_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb3_rn_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb2_rn_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb1_rn_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb4_rt_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb3_rt_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb2_rt_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb1_rt_4_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb4_rt_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb3_rt_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb2_rt_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dlarfb1_rt_1_lib4(int kmax, double *pV, double *pT, double *pD);
+void kernel_dgelqf_pd_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT);
+void kernel_dgelqf_pd_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT);
+void kernel_dgelqf_pd_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT);
+void kernel_dgelqf_pd_4_lib4(int n, double *pD, double *dD);
+void kernel_dgelqf_pd_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD);
+void kernel_dgelqf_pd_la_vs_lib4(int m, int n1, int k, int offD, double *pD, int sdd, double *dD, int offA, double *pA, int sda);
+void kernel_dlarft_4_la_lib4(int n1, double *dD, double *pA, double *pT);
+void kernel_dlarfb4_rn_12_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda);
+void kernel_dlarfb4_rn_8_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda);
+void kernel_dlarfb4_rn_4_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA);
+void kernel_dlarfb4_rn_1_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA);
+void kernel_dgelqf_pd_lla_vs_lib4(int m, int n0, int n1, int k, int offD, double *pD, int sdd, double *dD, int offL, double *pL, int sdl, int offA, double *pA, int sda);
+void kernel_dlarft_4_lla_lib4(int n0, int n1, double *dD, double *pL, double *pA, double *pT);
+void kernel_dlarfb4_rn_12_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda);
+void kernel_dlarfb4_rn_8_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda);
+void kernel_dlarfb4_rn_4_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA);
+void kernel_dlarfb4_rn_1_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA);
+// 4x2
+void kernel_dpotrf_nt_l_4x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D);
+void kernel_dpotrf_nt_l_4x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn);
+// 2x2
+void kernel_dpotrf_nt_l_2x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D);
+void kernel_dpotrf_nt_l_2x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn);
+// 12
+void kernel_dgetrf_pivot_12_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_12_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n);
+// 8
+void kernel_dgetrf_pivot_8_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_8_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n);
+// 4
+void kernel_dgetrf_pivot_4_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_4_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n1);
+// vector
+void kernel_drowsw_lib4(int kmax, double *pA, double *pC);
+
+
+
+// merged routines
+// 12x4
+void kernel_dgemm_dtrsm_nt_rl_inv_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dgemm_dtrsm_nt_rl_inv_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dsyrk_dpotrf_nt_l_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+// 4x12
+void kernel_dgemm_dtrsm_nt_rl_inv_4x12_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn);
+// 8x8
+void kernel_dsyrk_dpotrf_nt_l_8x8_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+void kernel_dgemm_dtrsm_nt_rl_inv_8x8l_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+void kernel_dgemm_dtrsm_nt_rl_inv_8x8u_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
+// 8x4
+void kernel_dgemm_dtrsm_nt_rl_inv_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dgemm_dtrsm_nt_rl_inv_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
+void kernel_dsyrk_dpotrf_nt_l_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
+void kernel_dsyrk_dpotrf_nt_l_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
+// 4x8
+void kernel_dgemm_dtrsm_nt_rl_inv_4x8_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn);
+// 4x4
+void kernel_dgemm_dtrsm_nt_rl_inv_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dgemm_dtrsm_nt_rl_inv_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
+// 4x2
+void kernel_dgemm_dtrsm_nt_rl_inv_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E);
+void kernel_dgemm_dtrsm_nt_rl_inv_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
+// 2x2
+void kernel_dsyrk_dpotrf_nt_l_2x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
+void kernel_dsyrk_dpotrf_nt_l_2x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
+
+/*
+ *
+ * Auxiliary routines
+ *
+ * cpsc copy and scale, scale
+ * cp copy
+ * add
+ * set and scale
+ * transpose and scale
+ * set and scale
+ *
+ */
+
+// copy and scale
+void kernel_dgecpsc_8_0_lib4(int tri, int kmax, double alpha, double *A0, int sda,  double *B, int sdb);
+void kernel_dgecpsc_8_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+void kernel_dgecpsc_8_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+void kernel_dgecpsc_8_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+
+void kernel_dgecpsc_4_0_lib4(int tri, int kmax, double alpha, double *A, double *B);
+void kernel_dgecpsc_4_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgecpsc_4_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgecpsc_4_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+
+void kernel_dgecpsc_3_0_lib4(int tri, int kmax, double alpha, double *A, double *B);
+void kernel_dgecpsc_3_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgecpsc_3_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+
+void kernel_dgecpsc_2_0_lib4(int tri, int kmax, double alpha, double *A, double *B);
+void kernel_dgecpsc_2_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B);
+
+void kernel_dgecpsc_1_0_lib4(int tri, int kmax, double alpha, double *A, double *B);
+
+// copy only
+void kernel_dgecp_8_0_lib4(int tri, int kmax, double *A, int sda, double *B, int sdb);
+
+void kernel_dgecp_4_0_lib4(int tri, int kmax, double *A, double *B);
+
+// add
+void kernel_dgead_8_0_lib4(int kmax, double alpha, double *A0, int sda,  double *B, int sdb);
+void kernel_dgead_8_1_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+void kernel_dgead_8_2_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+void kernel_dgead_8_3_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb);
+void kernel_dgead_4_0_lib4(int kmax, double alpha, double *A, double *B);
+void kernel_dgead_4_1_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_4_2_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_4_3_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_3_0_lib4(int kmax, double alpha, double *A, double *B);
+void kernel_dgead_3_2_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_3_3_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_2_0_lib4(int kmax, double alpha, double *A, double *B);
+void kernel_dgead_2_3_lib4(int kmax, double alpha, double *A0, int sda, double *B);
+void kernel_dgead_1_0_lib4(int kmax, double alpha, double *A, double *B);
+
+// set
+void kernel_dgeset_4_lib4(int kmax, double alpha, double *A);
+void kernel_dtrset_4_lib4(int kmax, double alpha, double *A);
+
+// traspose
+void kernel_dgetr_8_lib4(int tri, int kmax, int kna, double alpha, double *A, int sda, double *C, int sdc);
+void kernel_dgetr_4_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc);
+void kernel_dgetr_3_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc);
+void kernel_dgetr_2_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc);
+void kernel_dgetr_1_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc);
+void kernel_dgetr_4_0_lib4(int m, double *A, int sda, double *B);
+
+
+
+// pack
+// 12
+void kernel_dpack_nn_12_lib4(int kmax, double *A, int lda, double *B, int sdb);
+void kernel_dpack_nn_12_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1);
+void kernel_dpack_tt_12_lib4(int kmax, double *A, int lda, double *B, int sdb);
+// 8
+void kernel_dpack_nn_8_lib4(int kmax, double *A, int lda, double *B, int sdb);
+void kernel_dpack_nn_8_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1);
+void kernel_dpack_tt_8_lib4(int kmax, double *A, int lda, double *B, int sdb);
+// 4
+void kernel_dpack_nn_4_lib4(int kmax, double *A, int lda, double *B);
+void kernel_dpack_nn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1);
+void kernel_dpack_tn_4_p0_lib4(int kmax, double *A, int lda, double *B);
+void kernel_dpack_tn_4_lib4(int kmax, double *A, int lda, double *B);
+void kernel_dpack_tn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1);
+void kernel_dpack_tt_4_lib4(int kmax, double *A, int lda, double *B, int sdb);
+void kernel_dpack_tt_4_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1);
+// unpack
+// 12
+void kernel_dunpack_nn_12_lib4(int kmax, double *A, int sda, double *B, int ldb);
+void kernel_dunpack_nn_12_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1);
+void kernel_dunpack_tt_12_lib4(int kmax, double *A, int sda, double *B, int ldb);
+// 8
+void kernel_dunpack_nn_8_lib4(int kmax, double *A, int sda, double *B, int ldb);
+void kernel_dunpack_nn_8_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1);
+void kernel_dunpack_tt_8_lib4(int kmax, double *A, int sda, double *B, int ldb);
+// 4
+void kernel_dunpack_nn_4_lib4(int kmax, double *A, double *B, int ldb);
+void kernel_dunpack_nn_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1);
+void kernel_dunpack_nt_4_lib4(int kmax, double *A, double *B, int ldb);
+void kernel_dunpack_nt_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1);
+void kernel_dunpack_tt_4_lib4(int kmax, double *A, int sda, double *B, int ldb);
+
+// panel copy
+// 12
+void kernel_dpacp_nn_12_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb);
+void kernel_dpacp_nn_12_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1);
+// 8
+void kernel_dpacp_nn_8_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb);
+void kernel_dpacp_nn_8_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1);
+// 4
+void kernel_dpacp_nt_4_lib4(int kmax, double *A, int offsetB, double *B, int sdb);
+void kernel_dpacp_tn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B);
+void kernel_dpacp_nn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B);
+void kernel_dpacp_nn_4_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int m1);
+
+
+
+/************************************************
+* BLAS API kernels
+************************************************/
+
+//#if defined(BLAS_API)
+
+// A, B panel-major bs=4; C, D column-major
+// 12x4
+void kernel_dgemm_nt_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_12x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p);
+void kernel_dsyrk_nt_l_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_u_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_u_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_12x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_rl_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_ru_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_ru_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dpotrf_nt_l_12x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD);
+void kernel_dpotrf_nt_l_12x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1);
+// 4x12
+void kernel_dgemm_nt_4x12_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x12_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+// 8x8
+void kernel_dsyrk_nt_l_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_u_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_u_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dpotrf_nt_l_8x8_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD);
+void kernel_dpotrf_nt_l_8x8_vs_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1);
+// 8x4
+void kernel_dgemm_nt_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_8x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p);
+void kernel_dgemm_nt_8x4_p_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p);
+void kernel_dsyrk_nt_l_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_u_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_u_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_8x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_rl_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_ru_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_ru_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dpotrf_nt_l_8x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD);
+void kernel_dpotrf_nt_l_8x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1);
+// 4x8
+void kernel_dgemm_nt_4x8_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x8_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_dgemm_nt_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_l_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_u_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_u_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_4x4_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE);
+void kernel_dtrsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E);
+void kernel_dtrsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1);
+void kernel_dpotrf_nt_l_4x4_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD);
+void kernel_dpotrf_nt_l_4x4_vs_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1);
+// 4x2
+void kernel_dgemm_nt_4x2_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+
+// A panel-major bs=4; B, C, D column-major
+// 12x4
+void kernel_dgemm_nn_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_l_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nn_ll_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ll_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dgetrf_nn_l_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_l_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+void kernel_dgetrf_nn_m_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_m_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+void kernel_dgetrf_nn_r_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_r_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+// 4x12
+void kernel_dgemm_nn_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+// 8x4
+void kernel_dgemm_nn_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_l_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nn_ll_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ll_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dgetrf_nn_l_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_l_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+void kernel_dgetrf_nn_r_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_r_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+// 4x8
+void kernel_dgemm_nn_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_dgemm_nn_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dsyrk_nt_l_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dsyrk_nt_l_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nn_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nn_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nn_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nn_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nn_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dtrmm_nt_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd);
+void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1);
+void kernel_dtrsm_nn_ll_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ll_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1);
+void kernel_dgetrf_nn_4x4_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag);
+void kernel_dgetrf_nn_4x4_vs_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1);
+
+// B panel-major bs=4; A, C, D column-major
+// 12x4
+void kernel_dgemm_nt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x12
+void kernel_dgemm_nt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 8x4
+void kernel_dgemm_nt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x8
+void kernel_dgemm_nt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_dgemm_nt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+
+// A, C, D panel-major; B, E column-major
+// TODO merge with above
+// 12x4
+void kernel_dtrsm_nn_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+// 8x4
+void kernel_dtrsm_nn_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1);
+// 4x4
+void kernel_dtrsm_nn_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde);
+void kernel_dtrsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nn_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE);
+void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nn_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde);
+void kernel_dtrsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde);
+void kernel_dtrsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1);
+void kernel_dtrsm_nt_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE);
+void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1);
+void kernel_dtrsm_nt_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde);
+void kernel_dtrsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1);
+
+// A, B, C, D column-major
+// 12x4
+void kernel_dgemm_nn_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x12
+void kernel_dgemm_nn_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 8x4
+void kernel_dgemm_nn_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x8
+void kernel_dgemm_nn_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_dgemm_nn_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nn_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_nt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_nt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+void kernel_dgemm_tt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
+void kernel_dgemm_tt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
+
+// A column-major
+// 12
+void kernel_dgetrf_pivot_12_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_12_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n);
+// 8
+void kernel_dgetrf_pivot_8_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_8_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n);
+// 4
+void kernel_dgetrf_pivot_4_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv);
+void kernel_dgetrf_pivot_4_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n);
+
+// vector
+void kernel_ddot_11_lib(int n, double *x, double *y, double *res);
+void kernel_daxpy_11_lib(int n, double *alpha, double *x, double *y);
+void kernel_drowsw_lib(int kmax, double *pA, int lda, double *pC, int ldc);
+
+//#endif // BLAS_API
+
+
+
+// larger kernels
+// 12
+void kernel_dgemm_nt_12xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p);
+void kernel_dgemm_nt_12xn_pl_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p);
+// 8
+void kernel_dgemm_nt_8xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p);
+
+
+
+// aux
+void kernel_dvecld_inc1(int kmax, double *x);
+void kernel_dveccp_inc1(int kmax, double *x, double *y);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_D_KERNEL_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h
new file mode 100644
index 000000000..74c3fb5c0
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h
@@ -0,0 +1,69 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_I_AUX_EXT_DEP_H_
+#define BLASFEO_I_AUX_EXT_DEP_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#ifdef EXT_DEP
+
+// i_aux_extern_depend_lib
+void int_zeros(int **pA, int row, int col);
+void int_zeros_align(int **pA, int row, int col);
+void int_free(int *pA);
+void int_free_align(int *pA);
+void int_print_mat(int row, int col, int *A, int lda);
+int int_print_to_string_mat(char **buf_out, int row, int col, int *A, int lda);
+
+#endif // EXT_DEP
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_I_AUX_EXT_DEP_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h
new file mode 100644
index 000000000..6248853e2
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h
@@ -0,0 +1,57 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_M_AUX_H_
+#define BLASFEO_M_AUX_H_
+
+#include "blasfeo_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+void blasfeo_cvt_d2s_vec(int m, struct blasfeo_dvec *vd, int vdi, struct blasfeo_svec *vs, int vsi);
+void blasfeo_cvt_s2d_vec(int m, struct blasfeo_svec *vs, int vsi, struct blasfeo_dvec *vd, int vdi);
+void blasfeo_cvt_d2s_mat(int m, int n, struct blasfeo_dmat *Md, int mid, int nid, struct blasfeo_smat *Ms, int mis, int nis);
+void blasfeo_cvt_s2d_mat(int m, int n, struct blasfeo_smat *Ms, int mis, int nis, struct blasfeo_dmat *Md, int mid, int nid);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_M_AUX_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_memory.h b/third_party/acados/include/blasfeo/include/blasfeo_memory.h
new file mode 100644
index 000000000..da4e7fa09
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_memory.h
@@ -0,0 +1,62 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2020 by Gianluca Frison.                                                          *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+#ifndef BLASFEO_MEMORY_H_
+#define BLASFEO_MEMORY_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+int blasfeo_is_init();
+//
+void blasfeo_init();
+//
+void blasfeo_quit();
+//
+void *blasfeo_get_buffer();
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLASFEO_MEMORY_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_naming.h b/third_party/acados/include/blasfeo/include/blasfeo_naming.h
new file mode 100644
index 000000000..c289443b1
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_naming.h
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+/*
+ * ----------- Naming conventions
+ *
+ *  (precision)(data)
+ *
+ * 1) d(double)
+ *    s(single)
+ *
+ * 2) ge(general)
+ *    tr(triangular)
+ *    vec(vector)
+ *    row(row)
+ *    col(column)
+ *    dia(diagonal)
+ *
+ * 3) se(set)
+ *    cp(copy)
+ *    sc(scale)
+ *    ad(add)
+ *    tr(transpose)
+ *    in(insert)
+ *    ex(extract)
+ *    pe(premute)
+ *    sw(swap)
+ *
+ *    f(factorization)
+ *
+ *    lqf(LQ factorization)
+ *    qrf (factorization)
+ *    trf (LU factorization using partial pivoting with row interchanges.)
+ *
+ * 4) _l(lower) / _u(upper)
+ *    _lib8 (hp implementation, 8 rows kernel)
+ *    _lib4 (hp implementation, 4 rows kernel)
+ *    _lib0 (hp interface with reference implentation)
+ *    _lib (reference implementation)
+ *    _libref (reference implementation with dedicated namespace)
+ *
+ * 5) _sp(sparse)
+ *    _exp(exponential format)
+ */
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h b/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h
new file mode 100644
index 000000000..65a0c7e80
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h
@@ -0,0 +1,88 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Ian McInerney                                                                           *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_PROCESSOR_FEATURES_H_
+#define BLASFEO_PROCESSOR_FEATURES_H_
+
+/**
+ * Flags to indicate the different processor features
+ */
+enum
+{
+    // x86-64 CPU features
+    BLASFEO_PROCESSOR_FEATURE_AVX  = 0x0001,    /// AVX instruction set
+    BLASFEO_PROCESSOR_FEATURE_AVX2 = 0x0002,    /// AVX2 instruction set
+    BLASFEO_PROCESSOR_FEATURE_FMA  = 0x0004,    /// FMA instruction set
+    BLASFEO_PROCESSOR_FEATURE_SSE3 = 0x0008,    /// SSE3 instruction set
+
+    // ARM CPU features
+    BLASFEO_PROCESSOR_FEATURE_VFPv3  = 0x0100,  /// VFPv3 instruction set
+    BLASFEO_PROCESSOR_FEATURE_NEON   = 0x0100,  /// NEON instruction set
+    BLASFEO_PROCESSOR_FEATURE_VFPv4  = 0x0100,  /// VFPv4 instruction set
+    BLASFEO_PROCESSOR_FEATURE_NEONv2 = 0x0100,  /// NEONv2 instruction set
+} BLASFEO_PROCESSOR_FEATURES;
+
+/**
+ * Test the features that this processor provides against what the library was compiled with.
+ *
+ * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum)
+ * @return 0 if current processor doesn't support all features required for this library, 1 otherwise
+ */
+int blasfeo_processor_cpu_features( int* features );
+
+/**
+ * Test the features that this processor provides against what the library was compiled with.
+ *
+ * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum)
+ * @return 0 if current processor doesn't support all features required for this library, 1 otherwise
+ */
+void blasfeo_processor_library_features( int* features );
+
+/**
+ * Create a string listing the features the current processor supports.
+ *
+ * @param features - Flags from the BLASFEO_PROCESSOR_FEATURES enum indicating the features supported
+ * @param featureString - Character array to store the feature string in
+ */
+void blasfeo_processor_feature_string( int features, char* featureString );
+
+/**
+ * Get a string listing the processor features that this library version needs to run.
+ *
+ * @param featureString - Character array to store the feature string in
+ */
+void blasfeo_processor_library_string( char* featureString );
+
+#endif  // BLASFEO_PROCESSOR_FEATURES_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
new file mode 100644
index 000000000..539268849
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h
@@ -0,0 +1,166 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_AUX_H_
+#define BLASFEO_S_AUX_H_
+
+
+
+#include <stdlib.h>
+
+#include "blasfeo_s_aux_old.h"
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/************************************************
+* d_aux_lib.c
+************************************************/
+
+// returns the memory size (in bytes) needed for a smat
+size_t blasfeo_memsize_smat(int m, int n);
+size_t blasfeo_memsize_smat_ps(int ps, int m, int n);
+// returns the memory size (in bytes) needed for the diagonal of a smat
+size_t blasfeo_memsize_diag_smat(int m, int n);
+// returns the memory size (in bytes) needed for a svec
+size_t blasfeo_memsize_svec(int m);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory);
+void blasfeo_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory);
+// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated)
+void blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory);
+void blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_pack_u_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai);
+void blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+void blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+void blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi);
+//void s_cast_mat2strmat(float *A, struct blasfeo_smat *sA);
+//void s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA);
+//void s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa);
+
+// ge
+void blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj);
+float blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// tr
+void blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// dia
+void blasfeo_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi);
+void blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// row
+void blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+void blasfeo_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// col
+void blasfeo_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+void blasfeo_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// vec
+void blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi);
+void blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai);
+void blasfeo_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi);
+float blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
+void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
+void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
+void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
+void blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm);
+void blasfeo_svecnrm_2(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm);
+void blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi);
+void blasfeo_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi);
+
+
+
+/*
+* Explicitly panel-major matrix format
+*/
+
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_pm_memsize_smat(int ps, int m, int n);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_pm_create_smat(int ps, int m, int n, struct blasfeo_pm_smat *sA, void *memory);
+
+
+
+/*
+* Explicitly column-major matrix format
+*/
+
+// returns the memory size (in bytes) needed for a dmat
+size_t blasfeo_cm_memsize_smat(int m, int n);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_cm_create_smat(int m, int n, struct blasfeo_pm_smat *sA, void *memory);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_AUX_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h
new file mode 100644
index 000000000..5209d20d3
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h
@@ -0,0 +1,141 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_AUX_EXT_DEP_H_
+#define BLASFEO_S_AUX_EXT_DEP_H_
+
+
+
+#include <stdio.h>
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#ifdef EXT_DEP
+
+/************************************************
+* s_aux_extern_depend_lib.c
+************************************************/
+
+/* column-major matrices */
+
+// dynamically allocate row*col floats of memory and set accordingly a pointer to float; set allocated memory to zero
+void s_zeros(float **pA, int row, int col);
+// dynamically allocate row*col floats of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero
+void s_zeros_align(float **pA, int row, int col);
+// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero
+void s_zeros_align_bytes(float **pA, int size);
+// free the memory allocated by d_zeros
+void s_free(float *pA);
+// free the memory allocated by d_zeros_align or d_zeros_align_bytes
+void s_free_align(float *pA);
+// print a column-major matrix
+void s_print_mat(int m, int n, float *A, int lda);
+// print the transposed of a column-major matrix
+void s_print_tran_mat(int row, int col, float *A, int lda);
+// print to file a column-major matrix
+void s_print_to_file_mat(FILE *file, int row, int col, float *A, int lda);
+// print to file a column-major matrix in exponential format
+void s_print_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda);
+// print to string a column-major matrix
+void s_print_to_string_mat(char **buf_out, int row, int col, float *A, int lda);
+// print to file the transposed of a column-major matrix
+void s_print_tran_to_file_mat(FILE *file, int row, int col, float *A, int lda);
+// print to file the transposed of a column-major matrix in exponential format
+void s_print_tran_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda);
+// print in exponential notation a column-major matrix
+void s_print_exp_mat(int m, int n, float *A, int lda);
+// print in exponential notation the transposed of a column-major matrix
+void s_print_exp_tran_mat(int row, int col, float *A, int lda);
+
+/* strmat and strvec */
+
+// create a strmat for a matrix of size m*n by dynamically allocating memory
+void blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA);
+// create a strvec for a vector of size m by dynamically allocating memory
+void blasfeo_allocate_svec(int m, struct blasfeo_svec *sa);
+// free the memory allocated by blasfeo_allocate_dmat
+void blasfeo_free_smat(struct blasfeo_smat *sA);
+// free the memory allocated by blasfeo_allocate_dvec
+void blasfeo_free_svec(struct blasfeo_svec *sa);
+// print a strmat
+void blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print in exponential notation a strmat
+void blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print to file a strmat
+void blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print to file a strmat in exponential format
+void blasfeo_print_to_file_exp_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print to string a strmat
+void blasfeo_print_to_string_smat(char **buf_out, int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print the transpose of a strmat
+void blasfeo_print_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+// print a strvec
+void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai);
+// print in exponential notation a strvec
+void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai);
+// print to file a strvec
+void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+// print to string a strvec
+void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai);
+// print the transposed of a strvec
+void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+// print in exponential notation the transposed of a strvec
+void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+// print to file the transposed of a strvec
+void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+// print to string the transposed of a strvec
+void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai);
+
+#endif // EXT_DEP
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif  // BLASFEO_S_AUX_EXT_DEP_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h
new file mode 100644
index 000000000..6640e20a4
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h
@@ -0,0 +1,82 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_AUX_EXT_DEP_REF_H_
+#define BLASFEO_S_AUX_EXT_DEP_REF_H_
+
+#if defined(EXT_DEP)
+
+
+
+#include <stdio.h>
+
+#include "blasfeo_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// expose reference BLASFEO for testing
+// see blasfeo_s_aux_exp_dep.h for help
+
+void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_allocate_smat_ref(int m, int n, struct blasfeo_smat_ref *sA);
+void blasfeo_allocate_svec_ref(int m, struct blasfeo_svec_ref *sa);
+void blasfeo_free_smat_ref(struct blasfeo_smat_ref *sA);
+void blasfeo_free_svec_ref(struct blasfeo_svec_ref *sa);
+void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_print_exp_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_print_to_file_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_print_to_file_exp_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_print_to_string_smat_ref(char **buf_out, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj);
+void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif  // EXT_DEP
+
+#endif  // BLASFEO_S_AUX_EXT_DEP_REF_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h
new file mode 100644
index 000000000..5c6db37ba
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h
@@ -0,0 +1,64 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+// TODO remove
+//
+void strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb);
+void sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb);
+void sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+void strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+void strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+void sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd);
+void sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd);
+void sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x);
+void sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd);
+void sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd);
+void sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x);
+void sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd);
+void sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd);
+void srowin_lib(int kmax, float alpha, float *x, float *pD);
+void srowex_lib(int kmax, float alpha, float *pD, float *x);
+void srowad_lib(int kmax, float alpha, float *x, float *pD);
+void srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD);
+void srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD);
+void srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD);
+void srowsw_lib(int kmax, float *pA, float *pC);
+void scolin_lib(int kmax, float *x, int offset, float *pD, int sdd);
+void scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd);
+void scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd);
+void scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd);
+void scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+void svecin_libsp(int kmax, int *idx, float *x, float *y);
+void svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y);
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
new file mode 100644
index 000000000..f6e16a957
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h
@@ -0,0 +1,145 @@
+
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_AUX_REF_H_
+#define BLASFEO_S_AUX_REF_H_
+
+
+
+#include <stdlib.h>
+
+#include "blasfeo_s_aux_old.h"
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/************************************************
+* d_aux_lib.c
+************************************************/
+
+// returns the memory size (in bytes) needed for a smat
+size_t blasfeo_ref_memsize_smat(int m, int n);
+size_t blasfeo_ref_memsize_smat_ps(int ps, int m, int n);
+// returns the memory size (in bytes) needed for the diagonal of a smat
+size_t blasfeo_ref_memsize_diag_smat(int m, int n);
+// returns the memory size (in bytes) needed for a svec
+size_t blasfeo_ref_memsize_svec(int m);
+// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated)
+void blasfeo_ref_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory);
+void blasfeo_ref_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory);
+// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated)
+void blasfeo_ref_create_svec(int m, struct blasfeo_svec *sA, void *memory);
+void blasfeo_ref_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj);
+void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj);
+void blasfeo_ref_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai);
+void blasfeo_ref_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+void blasfeo_ref_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+void blasfeo_ref_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi);
+void ref_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA);
+void ref_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA);
+void ref_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa);
+
+// ge
+void blasfeo_ref_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj);
+float blasfeo_ref_sgeex1(struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// tr
+void blasfeo_ref_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// dia
+void blasfeo_ref_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// row
+void blasfeo_ref_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+void blasfeo_ref_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// col
+void blasfeo_ref_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+void blasfeo_ref_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void blasfeo_ref_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+void blasfeo_ref_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// vec
+void blasfeo_ref_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_ref_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai);
+void blasfeo_ref_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_ref_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+void blasfeo_ref_svecin1(float a, struct blasfeo_svec *sx, int xi);
+float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
+void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
+void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
+void blasfeo_ref_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm);
+void blasfeo_ref_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi);
+void blasfeo_ref_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_AUX_REF_H_
+
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h
new file mode 100644
index 000000000..08d9a14a6
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h
@@ -0,0 +1,177 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_AUX_TEST_H_
+#define BLASFEO_S_AUX_TEST_H_
+
+#include <stdio.h>
+
+#include "blasfeo_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/************************************************
+* d_aux_lib.c
+************************************************/
+
+int test_blasfeo_memsize_smat(int m, int n);
+int test_blasfeo_memsize_diag_smat(int m, int n);
+int test_blasfeo_memsize_svec(int m);
+
+void test_blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory);
+void test_blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory);
+
+void test_blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void test_blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai);
+void test_blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj);
+void test_blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+void test_blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi);
+void test_blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda);
+
+void test_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA);
+void test_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA);
+void test_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa);
+// copy and scale
+void test_blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void test_blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+void test_blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+
+// void test_blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj);
+// float test_blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj);
+// void test_blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi);
+// float test_blasfeo_svecex1(struct blasfeo_svec *sx, int xi);
+
+// // A <= alpha
+// void test_blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj);
+// // a <= alpha
+// void test_blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi);
+
+
+// void test_blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+// void test_blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai);
+
+// void test_strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb);
+// void test_blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+
+// void test_sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb);
+// void test_blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// void test_blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci);
+
+// void test_sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+// void test_blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+
+// void test_strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+// void test_blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// void test_strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+// void test_blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+
+// void test_sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd);
+// void test_blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+// void test_blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+// void test_sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd);
+// void test_sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x);
+// void test_sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd);
+// void test_sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd);
+// void test_blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// void test_sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x);
+// void test_blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi);
+// void test_blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+// void test_sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd);
+// void test_blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// void test_sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd);
+// void test_blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// void test_srowin_lib(int kmax, float alpha, float *x, float *pD);
+// void test_blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+// void test_srowex_lib(int kmax, float alpha, float *pD, float *x);
+// void test_blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi);
+// void test_srowad_lib(int kmax, float alpha, float *x, float *pD);
+// void test_blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+// void test_srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD);
+// void test_srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD);
+// void test_blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj);
+// void test_srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD);
+// void test_srowsw_lib(int kmax, float *pA, float *pC);
+// void test_blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// void test_blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// void test_scolin_lib(int kmax, float *x, int offset, float *pD, int sdd);
+// void test_blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj);
+// void test_scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd);
+// void test_scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd);
+// void test_scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd);
+// void test_scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc);
+// void test_blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj);
+// void test_blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA);
+// void test_svecin_libsp(int kmax, int *idx, float *x, float *y);
+// void test_svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y);
+// void test_blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+// void test_blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi);
+// void test_blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi);
+// void test_blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi);
+// void test_blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi);
+// void test_blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei);
+// void test_blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm);
+// void test_blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi);
+
+
+// ext_dep
+
+void test_blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA);
+void test_blasfeo_allocate_svec(int m, struct blasfeo_svec *sa);
+
+void test_blasfeo_free_smat(struct blasfeo_smat *sA);
+void test_blasfeo_free_svec(struct blasfeo_svec *sa);
+
+void test_blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+void test_blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai);
+void test_blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+
+void test_blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+void test_blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+void test_blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai);
+
+void test_blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj);
+void test_blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai);
+void test_blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_AUX_TEST_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h
new file mode 100644
index 000000000..200f1f51b
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h
@@ -0,0 +1,46 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_BLAS_H_
+#define BLASFEO_S_BLAS_H_
+
+
+
+#include "blasfeo_s_blasfeo_api.h"
+#include "blasfeo_s_blas_api.h"
+
+
+
+#endif  // BLASFEO_S_BLAS_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h
new file mode 100644
index 000000000..d0fa2245c
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h
@@ -0,0 +1,117 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef BLASFEO_S_BLAS_API_H_
+#define BLASFEO_S_BLAS_API_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#ifdef BLAS_API
+
+
+
+#ifdef FORTRAN_BLAS_API
+
+
+
+// BLAS 1
+//
+void saxpy_(int *n, float *alpha, float *x, int *incx, float *y, int *incy);
+//
+float sdot_(int *n, float *x, int *incx, float *y, int *incy);
+
+// BLAS 3
+//
+void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc);
+//
+void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb);
+
+
+
+// LAPACK
+//
+void spotrf_(char *uplo, int *m, float *A, int *lda, int *info);
+
+
+
+#else // BLASFEO_API
+
+
+
+// BLAS 1
+//
+void blas_saxpy(int *n, float *alpha, float *x, int *incx, float *y, int *incy);
+//
+float blas_sdot(int *n, float *x, int *incx, float *y, int *incy);
+
+// BLAS 3
+//
+void blas_sgemm(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc);
+//
+void blas_strsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb);
+
+
+
+// LAPACK
+//
+void blas_spotrf(char *uplo, int *m, float *A, int *lda, int *info);
+
+
+
+#endif // BLASFEO_API
+
+
+
+#endif // BLAS_API
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_BLAS_API_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h
new file mode 100644
index 000000000..72c19dcdc
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h
@@ -0,0 +1,268 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_BLASFEO_API_H_
+#define BLASFEO_S_BLASFEO_API_H_
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+// level 1 BLAS
+//
+
+// z = y + alpha*x
+void blasfeo_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = beta*y + alpha*x
+void blasfeo_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = x .* y
+void blasfeo_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z += x .* y
+void blasfeo_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = x .* y, return sum(z) = x^T * y
+float blasfeo_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// return x^T * y
+float blasfeo_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi);
+// construct givens plane rotation
+void blasfeo_srotg(float a, float b, float *c, float *s);
+// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai
+void blasfeo_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s);
+// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj
+void blasfeo_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s);
+
+
+
+//
+// level 2 BLAS
+//
+
+// dense
+
+// z <= beta * y + alpha * A * x
+void blasfeo_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z <= beta * y + alpha * A' * x
+void blasfeo_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(n)
+void blasfeo_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(n)
+void blasfeo_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit
+void blasfeo_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit
+void blasfeo_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit
+void blasfeo_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit
+void blasfeo_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit
+void blasfeo_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
+void blasfeo_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= beta * y + alpha * A * x ; A upper triangular
+void blasfeo_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A' * x ; A upper triangular
+void blasfeo_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A * x ; A lower triangular
+void blasfeo_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A' * x ; A lower triangular
+void blasfeo_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z_n <= beta_n * y_n + alpha_n * A  * x_n
+// z_t <= beta_t * y_t + alpha_t * A' * x_t
+void blasfeo_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t);
+// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
+void blasfeo_ssymv_l(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+void blasfeo_ssymv_l_mn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+
+// diagonal
+
+// z <= beta * y + alpha * A * x, A diagonal
+void blasfeo_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+
+
+
+//
+// level 3 BLAS
+//
+
+// dense
+
+// D <= beta * C + alpha * A * B
+void blasfeo_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T
+void blasfeo_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D lower triangular
+void blasfeo_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D lower triangular
+void blasfeo_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D upper triangular
+void blasfeo_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D upper triangular
+void blasfeo_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^T ; B upper triangular
+void blasfeo_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal
+void blasfeo_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal
+void blasfeo_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal
+void blasfeo_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal
+void blasfeo_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal
+void blasfeo_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal
+void blasfeo_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal
+void blasfeo_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal
+void blasfeo_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+
+// diagonal
+
+// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec)
+void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd);
+void blasfeo_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec)
+void blasfeo_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+
+
+
+//
+// LAPACK
+//
+
+// D <= chol( C ) ; C, D lower triangular
+void blasfeo_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= chol( C ) ; C, D upper triangular
+void blasfeo_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= chol( C + A * B' ) ; C, D lower triangular
+void blasfeo_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= lu( C ) ; no pivoting
+void blasfeo_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= lu( C ) ; row pivoting
+void blasfeo_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv);
+// D <= qr( C )
+void blasfeo_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+int blasfeo_sgeqrf_worksize(int m, int n); // in bytes
+// D <= Q factor, where C is the output of the LQ factorization
+int blasfeo_sorglq_worksize(int m, int n, int k); // in bytes
+void blasfeo_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+// D <= lq( C )
+void blasfeo_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+int blasfeo_sgelqf_worksize(int m, int n); // in bytes
+// D <= lq( C ), positive diagonal elements
+void blasfeo_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work);
+// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with:
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work);
+
+
+
+
+//
+// BLAS API helper functions
+//
+
+#if ( defined(BLAS_API) & defined(MF_PANELMAJ) )
+// BLAS 3
+void blasfeo_cm_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_llnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_llnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lunn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lunu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_lutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rlnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rlnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_runn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_runu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_strsm_rutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj);
+// LAPACK
+void blasfeo_cm_spotrf_l(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+void blasfeo_cm_spotrf_u(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj);
+#endif
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_BLASFEO_API_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h
new file mode 100644
index 000000000..f429a79dc
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h
@@ -0,0 +1,135 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_BLASFEO_API_REF_H_
+#define BLASFEO_S_BLASFEO_API_REF_H_
+
+#include "blasfeo_common.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+// expose reference BLASFEO for testing
+
+// --- level 1
+
+void blasfeo_saxpy_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_saxpby_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_svecmul_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_svecmulacc_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+float blasfeo_svecmuldot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+float blasfeo_sdot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi);
+void blasfeo_srotg_ref(float a, float b, float *c, float *s);
+void blasfeo_scolrot_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj0, int aj1, float c, float s);
+void blasfeo_srowrot_ref(int m, struct blasfeo_smat_ref *sA, int ai0, int ai1, int aj, float c, float s);
+
+
+// --- level 2
+
+// dense
+void blasfeo_sgemv_n_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_sgemv_t_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_lnn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_ltn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_lnn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_lnu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_ltn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_ltu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strsv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_lnn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_ltn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_lnu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_strmv_ltu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi);
+void blasfeo_sgemv_nt_ref(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx_n, int xi_n, struct blasfeo_svec_ref *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec_ref *sy_n, int yi_n, struct blasfeo_svec_ref *sy_t, int yi_t, struct blasfeo_svec_ref *sz_n, int zi_n, struct blasfeo_svec_ref *sz_t, int zi_t);
+void blasfeo_ssymv_l_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+
+// diagonal
+void blasfeo_sgemv_d_ref(int m, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi);
+
+
+// --- level 3
+
+// dense
+void blasfeo_sgemm_nn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgemm_nt_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgemm_tn_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgemm_tt_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+
+void blasfeo_ssyrk_ln_mn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_ln_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_lt_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_un_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_ut_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+
+void blasfeo_strmm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strmm_rlnn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strsm_rltn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strsm_rltu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strsm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strsm_llnu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_strsm_lunn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj);
+
+// diagonal
+void dgemm_diag_left_lib_ref(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd);
+void blasfeo_sgemm_dn_ref(int m, int n, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgemm_nd_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sB, int bi, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+
+// --- lapack
+
+void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv);
+void blasfeo_spotrf_l_ref(int m, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_spotrf_l_mn_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_ssyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj);
+void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv);
+void blasfeo_sgeqrf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work);
+void blasfeo_sgelqf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work);
+void blasfeo_sgelqf_pd_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work);
+void blasfeo_sgelqf_pd_la_ref(int m, int n1, struct blasfeo_smat_ref *sL, int li, int lj, struct blasfeo_smat_ref *sA, int ai, int aj, void *work);
+void blasfeo_sgelqf_pd_lla_ref(int m, int n1, struct blasfeo_smat_ref *sL0, int l0i, int l0j, struct blasfeo_smat_ref *sL1, int l1i, int l1j, struct blasfeo_smat_ref *sA, int ai, int aj, void *work);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_BLASFEO_API_REF_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h
new file mode 100644
index 000000000..db8b28d3b
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h
@@ -0,0 +1,235 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_BLASFEO_REF_API_H_
+#define BLASFEO_S_BLASFEO_REF_API_H_
+
+
+
+#include "blasfeo_common.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+// level 1 BLAS
+//
+
+// z = y + alpha*x
+void blasfeo_ref_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = beta*y + alpha*x
+void blasfeo_ref_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = x .* y
+void blasfeo_ref_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z += x .* y
+void blasfeo_ref_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z = x .* y, return sum(z) = x^T * y
+float blasfeo_ref_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// return x^T * y
+float blasfeo_ref_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi);
+// construct givens plane rotation
+void blasfeo_ref_srotg(float a, float b, float *c, float *s);
+// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai
+void blasfeo_ref_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s);
+// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj
+void blasfeo_ref_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s);
+
+
+
+//
+// level 2 BLAS
+//
+
+// dense
+
+// z <= beta * y + alpha * A * x
+void blasfeo_ref_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z <= beta * y + alpha * A' * x
+void blasfeo_ref_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(n)
+void blasfeo_ref_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(n)
+void blasfeo_ref_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit
+void blasfeo_ref_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit
+void blasfeo_ref_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit
+void blasfeo_ref_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit
+void blasfeo_ref_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit
+void blasfeo_ref_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
+void blasfeo_ref_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= beta * y + alpha * A * x ; A upper triangular
+void blasfeo_ref_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A' * x ; A upper triangular
+void blasfeo_ref_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A * x ; A lower triangular
+void blasfeo_ref_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z <= A' * x ; A lower triangular
+void blasfeo_ref_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
+// z_n <= beta_n * y_n + alpha_n * A  * x_n
+// z_t <= beta_t * y_t + alpha_t * A' * x_t
+void blasfeo_ref_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t);
+// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
+void blasfeo_ref_ssymv_l(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+
+// diagonal
+
+// z <= beta * y + alpha * A * x, A diagonal
+void blasfeo_ref_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
+
+
+
+//
+// level 3 BLAS
+//
+
+// dense
+
+// D <= beta * C + alpha * A * B
+void blasfeo_ref_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T
+void blasfeo_ref_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_ref_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B
+void blasfeo_ref_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D lower triangular
+void blasfeo_ref_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D lower triangular
+void blasfeo_ref_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A * B^T ; C, D upper triangular
+void blasfeo_ref_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= beta * C + alpha * A^T * B ; C, D upper triangular
+void blasfeo_ref_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^T ; B upper triangular
+void blasfeo_ref_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A ; A lower triangular
+void blasfeo_ref_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_ref_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal
+void blasfeo_ref_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal
+void blasfeo_ref_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal
+void blasfeo_ref_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal
+void blasfeo_ref_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal
+void blasfeo_ref_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal
+void blasfeo_ref_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal
+void blasfeo_ref_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal
+void blasfeo_ref_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal
+void blasfeo_ref_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal
+void blasfeo_ref_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj);
+
+// diagonal
+
+// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec)
+void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd);
+void blasfeo_ref_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec)
+void blasfeo_ref_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+
+
+
+//
+// LAPACK
+//
+
+// D <= chol( C ) ; C, D lower triangular
+void blasfeo_ref_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= chol( C ) ; C, D upper triangular
+void blasfeo_ref_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= chol( C + A * B' ) ; C, D lower triangular
+void blasfeo_ref_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+void blasfeo_ref_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= lu( C ) ; no pivoting
+void blasfeo_ref_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj);
+// D <= lu( C ) ; row pivoting
+void blasfeo_ref_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv);
+// D <= qr( C )
+void blasfeo_ref_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+int blasfeo_ref_sgeqrf_worksize(int m, int n); // in bytes
+// D <= Q factor, where C is the output of the LQ factorization
+int blasfeo_ref_sorglq_worksize(int m, int n, int k); // in bytes
+void blasfeo_ref_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+// D <= lq( C )
+void blasfeo_ref_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+int blasfeo_ref_sgelqf_worksize(int m, int n); // in bytes
+// D <= lq( C ), positive diagonal elements
+void blasfeo_ref_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work);
+// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_ref_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work);
+// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with:
+// L lower triangular, of size (m)x(m)
+// A full, of size (m)x(n1)
+void blasfeo_ref_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_BLASFEO_REF_API_H_
+
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h b/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h
new file mode 100644
index 000000000..dcbde989c
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h
@@ -0,0 +1,682 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_S_KERNEL_H_
+#define BLASFEO_S_KERNEL_H_
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// utils
+void blasfeo_align_2MB(void *ptr, void **ptr_align);
+void blasfeo_align_4096_byte(void *ptr, void **ptr_align);
+void blasfeo_align_64_byte(void *ptr, void **ptr_align);
+
+
+
+//
+// lib8
+//
+
+// 24x4
+void kernel_sgemm_nt_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_nt_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_sgemm_nt_24x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_sgemm_nn_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_nn_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_sgemm_nn_24x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_ssyrk_nt_l_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_ssyrk_nt_l_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_ssyrk_nt_l_20x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_ssyrk_nt_l_20x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_spotrf_nt_l_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_spotrf_nt_l_20x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_20x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_strsm_nt_rl_inv_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn);
+void kernel_sgemm_strsm_nt_rl_inv_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_sgemm_strsm_nt_rl_inv_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_20x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_20x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_ssyrk_spotrf_nt_l_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_strmm_nn_rl_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd);
+void kernel_strmm_nn_rl_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn);
+
+// 16x8
+void kernel_sgemm_nt_16x8_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *spil);
+
+// 16x4
+void kernel_sgemm_nt_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_nt_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_sgemm_nt_16x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_sgemm_nn_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_nn_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_sgemm_nn_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_ssyrk_nt_l_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_ssyrk_nt_l_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_ssyrk_nt_l_12x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_ssyrk_nt_l_12x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn);
+void kernel_spotrf_nt_l_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_spotrf_nt_l_12x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_12x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_strsm_nt_rl_inv_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn);
+void kernel_sgemm_strsm_nt_rl_inv_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_sgemm_strsm_nt_rl_inv_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_12x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_12x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_ssyrk_spotrf_nt_l_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_strmm_nn_rl_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd);
+void kernel_strmm_nn_rl_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn);
+void kernel_strmm_nn_rl_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+
+// 8x8
+void kernel_sgemm_nt_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_nt_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn);
+void kernel_sgemm_nt_8x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_sgemm_nn_8x8_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D);
+void kernel_sgemm_nn_8x8_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn);
+void kernel_sgemm_nn_8x8_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_ssyrk_nt_l_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_ssyrk_nt_l_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn);
+void kernel_spotrf_nt_l_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D);
+void kernel_spotrf_nt_l_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_strsm_nt_rl_inv_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_sgemm_strsm_nt_rl_inv_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_sgemm_strsm_nt_rl_inv_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D);
+
+// 8x4
+void kernel_sgemm_nt_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_nt_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn);
+void kernel_sgemm_nt_8x4_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_sgemm_nn_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D);
+void kernel_sgemm_nn_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn);
+void kernel_sgemm_nn_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+//void kernel_ssyrk_nt_l_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_ssyrk_nt_l_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn);
+void kernel_spotrf_nt_l_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D);
+void kernel_spotrf_nt_l_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_strsm_nt_rl_inv_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_sgemm_strsm_nt_rl_inv_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_sgemm_strsm_nt_rl_inv_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D);
+void kernel_strmm_nn_rl_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D);
+void kernel_strmm_nn_rl_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D, int km, int kn);
+void kernel_strmm_nn_rl_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_strmm_nt_ru_8x4_lib8(int k, float *alpha, float *A, float *B, float *D);
+void kernel_strmm_nt_ru_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *D, int km, int kn);
+
+// 4x8
+void kernel_sgemm_nt_4x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_nt_4x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn);
+void kernel_sgemm_nt_4x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_strsm_nt_rl_inv_4x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_4x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+
+// 8
+void kernel_sgemv_n_8_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z);
+void kernel_sgemv_n_8_vs_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1);
+void kernel_sgemv_n_8_gen_lib8(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1);
+void kernel_sgemv_t_8_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z);
+void kernel_sgemv_t_8_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1);
+void kernel_sgemv_t_4_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z);
+void kernel_sgemv_t_4_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1);
+void kernel_strsv_ln_inv_8_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_ln_inv_8_vs_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn);
+void kernel_strsv_lt_inv_8_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_lt_inv_8_vs_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z, int km, int kn);
+void kernel_sgemv_nt_4_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t);
+void kernel_sgemv_nt_4_vs_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km);
+void kernel_ssymv_l_4l_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z);
+void kernel_ssymv_l_4r_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z);
+void kernel_ssymv_l_4l_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km);
+void kernel_ssymv_l_4r_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km);
+
+// -------- aux
+
+// ---- copy
+
+// lib4
+//
+void kernel_sgecpsc_4_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgecp_4_0_lib4(int kmax, float *A, float *B);
+
+void kernel_sgecpsc_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_4_1_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_sgecpsc_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_4_2_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_sgecpsc_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_4_3_lib4(int kmax, float *A0, int sda, float *B);
+
+void kernel_sgecpsc_3_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgecp_3_0_lib4(int kmax, float *A, float *B);
+void kernel_sgecpsc_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_3_2_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_sgecpsc_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_3_3_lib4(int kmax, float *A0, int sda, float *B);
+
+void kernel_sgecpsc_2_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgecp_2_0_lib4(int kmax, float *A, float *B);
+void kernel_sgecpsc_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgecp_2_3_lib4(int kmax, float *A0, int sda, float *B);
+
+void kernel_sgecpsc_1_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgecp_1_0_lib4(int kmax, float *A, float *B);
+
+// lib8
+//
+void kernel_sgecp_8_0_lib8(int m, float *A, float *B);
+void kernel_sgecp_8_0_gen_lib8(int m, float *A, float *B, int m1);
+void kernel_sgecp_8_0_gen_u_lib8(int m, float *A, float *B, int m1);
+
+void kernel_sgesc_8_0_lib8(int m, float *alpha, float *A);
+void kernel_sgesc_8_0_gen_lib8(int m, float *alpha, float *A, int m1);
+void kernel_sgesc_8_0_gen_u_lib8(int m, float *alpha, float *A, int m1);
+
+void kernel_sgecpsc_8_0_lib8(int m, float *alpha, float *A, float *B);
+void kernel_sgecpsc_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1);
+void kernel_sgecpsc_8_0_gen_u_lib8(int m, float *alpha, float *A, float *B, int m1);
+
+void kernel_sgecp_8_1_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_1_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_2_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_2_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_3_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_3_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_4_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_4_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_5_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_5_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_6_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_6_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+void kernel_sgecp_8_7_lib8(int m, float *A, int sda, float *B);
+void kernel_sgecp_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgecpsc_8_7_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgecpsc_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+// transpose
+//
+void kernel_sgetr_8_0_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_0_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_1_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_2_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_3_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_4_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_5_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_6_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1);
+void kernel_sgetr_8_7_lib8(int m, float *A, int sda, float *B);
+void kernel_sgetr_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1);
+
+// add
+//
+void kernel_sgead_8_0_lib8(int m, float *alpha, float *A, float *B);
+void kernel_sgead_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1);
+void kernel_sgead_8_1_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_2_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_3_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_4_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_5_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_6_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+void kernel_sgead_8_7_lib8(int m, float *alpha, float *A, int sda, float *B);
+void kernel_sgead_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1);
+
+
+//
+// lib4
+//
+
+
+
+// level 2 BLAS
+// 4
+void kernel_sgemv_n_4_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z);
+void kernel_sgemv_n_4_vs_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1);
+void kernel_sgemv_n_4_gen_lib4(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1);
+void kernel_sgemv_t_4_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z);
+void kernel_sgemv_t_4_vs_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1);
+void kernel_strsv_ln_inv_4_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_ln_inv_4_vs_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn);
+void kernel_strsv_lt_inv_4_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_lt_inv_3_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_lt_inv_2_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strsv_lt_inv_1_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z);
+void kernel_strmv_un_4_lib4(int k, float *A, float *x, float *z);
+void kernel_strmv_ut_4_lib4(int k, float *A, int sda, float *x, float *z);
+void kernel_strmv_ut_4_vs_lib4(int k, float *A, int sda, float *x, float *z, int km);
+void kernel_sgemv_nt_6_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t);
+void kernel_sgemv_nt_4_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t);
+void kernel_sgemv_nt_4_vs_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km);
+void kernel_ssymv_l_4_lib4(int kmax, float *alpha, float *A, int sda, float *x_n, float *z_n);
+void kernel_ssymv_l_4_gen_lib4(int kmax, float *alpha, int offA, float *A, int sda, float *x_n, float *z_n, int km);
+
+
+
+// level 3 BLAS
+// 12x4
+void kernel_sgemm_nt_16x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nt_16x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+void kernel_strsm_nt_rl_inv_16x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_16x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1);
+// 12x4
+void kernel_sgemm_nt_12x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nt_12x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+void kernel_strsm_nt_rl_inv_12x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_12x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1);
+// 8x8
+void kernel_sgemm_nt_8x8_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nt_8x8_vs_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+void kernel_sgemm_nn_8x8_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nn_8x8_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+// 8x4
+void kernel_sgemm_nt_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nt_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+void kernel_sgemm_nn_8x4_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_sgemm_nn_8x4_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); //
+void kernel_ssyrk_nt_l_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); //
+void kernel_ssyrk_nt_l_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); //
+void kernel_strsm_nt_rl_inv_8x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_8x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1);
+// 4x4
+void kernel_sgemm_nt_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); //
+void kernel_sgemm_nt_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); //
+void kernel_sgemm_nt_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int k0, int k1);
+void kernel_sgemm_nn_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); //
+void kernel_sgemm_nn_4x4_vs_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); //
+void kernel_sgemm_nn_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); //
+void kernel_ssyrk_nt_l_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); //
+void kernel_ssyrk_nt_l_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); //
+void kernel_ssyrk_nt_l_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_strmm_nt_ru_4x4_lib4(int k, float *alpha, float *A, float *B, float *D); //
+void kernel_strmm_nt_ru_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *D, int km, int kn); //
+void kernel_strmm_nn_rl_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D);
+void kernel_strmm_nn_rl_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1);
+void kernel_strsm_nt_rl_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nt_rl_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_strsm_nt_rl_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E);
+void kernel_strsm_nt_rl_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn);
+void kernel_strsm_nt_ru_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nt_ru_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_strsm_nt_ru_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E);
+void kernel_strsm_nt_ru_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn);
+void kernel_strsm_nn_ru_inv_4x4_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nn_ru_inv_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_strsm_nn_ll_one_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E);
+void kernel_strsm_nn_ll_one_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E, int km, int kn);
+void kernel_strsm_nn_lu_inv_4x4_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_strsm_nn_lu_inv_4x4_vs_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+// diag
+void kernel_sgemm_diag_right_4_a0_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *D, int sdd);
+void kernel_sgemm_diag_right_4_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_diag_right_3_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_diag_right_2_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_diag_right_1_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd);
+void kernel_sgemm_diag_left_4_a0_lib4(int kmax, float *alpha, float *A, float *B, float *D);
+void kernel_sgemm_diag_left_4_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_diag_left_3_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_diag_left_2_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+void kernel_sgemm_diag_left_1_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D);
+
+
+
+// LAPACK
+// 16x4
+void kernel_spotrf_nt_l_16x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_16x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1);
+// 12x4
+void kernel_spotrf_nt_l_12x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_12x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1);
+// 8x4
+void kernel_spotrf_nt_l_8x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D);
+void kernel_spotrf_nt_l_8x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1);
+// 4x4
+void kernel_spotrf_nt_l_4x4_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D);
+void kernel_spotrf_nt_l_4x4_vs_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_sgetrf_nn_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D);
+void kernel_sgetrf_nn_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_sgetrf_pivot_4_lib4(int m, float *pA, int sda, float *inv_diag_A, int* ipiv);
+void kernel_sgetrf_pivot_4_vs_lib4(int m, int n, float *pA, int sda, float *inv_diag_A, int* ipiv);
+
+
+
+// merged routines
+// 4x4
+void kernel_sgemm_strsm_nt_rl_inv_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E);
+void kernel_sgemm_strsm_nt_rl_inv_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn);
+void kernel_ssyrk_spotrf_nt_l_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D);
+
+
+
+// auxiliary routines
+void kernel_strcp_l_4_0_lib4(int kmax, float *A, float *B);
+void kernel_strcp_l_4_1_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_4_2_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_4_3_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_3_0_lib4(int kmax, float *A, float *B);
+void kernel_strcp_l_3_2_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_3_3_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_2_0_lib4(int kmax, float *A, float *B);
+void kernel_strcp_l_2_3_lib4(int kmax, float *A0, int sda, float *B);
+void kernel_strcp_l_1_0_lib4(int kmax, float *A, float *B);
+void kernel_sgead_4_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgead_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_3_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgead_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_2_0_lib4(int kmax, float *alpha, float *A, float *B);
+void kernel_sgead_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B);
+void kernel_sgead_1_0_lib4(int kmax, float *alpha, float *A, float *B);
+// TODO
+void kernel_sgeset_4_lib4(int kmax, float alpha, float *A);
+void kernel_strset_4_lib4(int kmax, float alpha, float *A);
+void kernel_sgetr_4_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc);
+void kernel_sgetr_3_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc);
+void kernel_sgetr_2_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc);
+void kernel_sgetr_1_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc);
+
+
+
+// pack
+// 24 lib 8
+void kernel_spack_nn_24_lib8(int kmax, float *A, int lda, float *B, int sdb);
+void kernel_spack_nn_24_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1);
+// 16 lib 8
+void kernel_spack_nn_16_lib8(int kmax, float *A, int lda, float *B, int sdb);
+void kernel_spack_nn_16_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1);
+// 8 lib 8
+void kernel_spack_nn_8_lib8(int kmax, float *A, int lda, float *B);
+void kernel_spack_nn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1);
+void kernel_spack_tn_8_lib8(int kmax, float *A, int lda, float *B);
+void kernel_spack_tn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1);
+void kernel_spack_tt_8_lib8(int kmax, float *A, int lda, float *B, int sdb);
+void kernel_spack_tt_8_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1);
+// 8 lib 4
+void kernel_spack_nn_8_lib4(int kmax, float *A, int lda, float *B, int sdb);
+void kernel_spack_nn_8_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1);
+//void kernel_spack_tt_8_lib4(int kmax, float *A, int lda, float *B, int sdb);
+// 4
+void kernel_spack_nn_4_lib4(int kmax, float *A, int lda, float *B);
+void kernel_spack_nn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1);
+void kernel_spack_tn_4_lib4(int kmax, float *A, int lda, float *B);
+void kernel_spack_tn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1);
+void kernel_spack_tt_4_lib4(int kmax, float *A, int lda, float *B, int sdb);
+void kernel_spack_tt_4_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1);
+// unpack
+// 8
+void kernel_sunpack_nn_8_lib4(int kmax, float *A, int sda, float *B, int ldb);
+void kernel_sunpack_nn_8_vs_lib4(int kmax, float *A, int sda, float *B, int ldb, int m1);
+//void kernel_sunpack_tt_8_lib4(int kmax, float *A, int sda, float *B, int ldb);
+// 4
+void kernel_sunpack_nn_4_lib4(int kmax, float *A, float *B, int ldb);
+void kernel_sunpack_nn_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1);
+void kernel_sunpack_nt_4_lib4(int kmax, float *A, float *B, int ldb);
+void kernel_sunpack_nt_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1);
+void kernel_sunpack_tt_4_lib4(int kmax, float *A, int sda, float *B, int ldb);
+
+// panel copy
+// 4
+void kernel_spacp_nt_4_lib4(int kmax, float *A, int offsetB, float *B, int sdb);
+void kernel_spacp_tn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B);
+void kernel_spacp_nn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B);
+void kernel_spacp_nn_4_vs_lib4(int kmax, int offsetA, float *A, int sda, float *B, int m1);
+
+
+
+/************************************************
+* BLAS API kernels
+************************************************/
+
+//#if defined(BLAS_API)
+
+// A, B panel-major bs=8; C, D column-major
+// 24x4
+void kernel_sgemm_nt_24x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_24x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 16x4
+void kernel_sgemm_nt_16x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_16x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 8x8
+void kernel_sgemm_nt_8x8_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x8_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_spotrf_nt_l_8x8_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD);
+void kernel_spotrf_nt_l_8x8_vs_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1);
+void kernel_strsm_nt_rl_inv_8x8_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nt_rl_inv_8x8_vs_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+// 8x4
+void kernel_sgemm_nt_8x4_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x4_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+
+// A, B panel-major bs=4; C, D column-major
+// 8x8
+void kernel_sgemm_nt_8x8_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
+// 8x4
+void kernel_sgemm_nt_8x4_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x4_vs_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_spotrf_nt_l_8x4_lib44cc(int kmax, float *A, int sda, float *B, float *C, int ldc, float *D, int ldd, float *dD);
+void kernel_strsm_nt_rl_inv_8x4_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+// 4x4
+void kernel_sgemm_nt_4x4_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x4_vs_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_strsm_nt_rl_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE);
+void kernel_strsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1);
+void kernel_strsm_nt_rl_inv_4x4_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nt_rl_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E);
+void kernel_strsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1);
+void kernel_strsm_nt_ru_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE);
+void kernel_strsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1);
+void kernel_strsm_nt_ru_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E);
+void kernel_strsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1);
+void kernel_spotrf_nt_l_4x4_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD);
+void kernel_spotrf_nt_l_4x4_vs_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1);
+
+// B panel-major bs=8; A, C, D column-major
+// 8x8
+void kernel_sgemm_nt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 4x8
+void kernel_sgemm_nt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+
+// B panel-major bs=4; A, C, D column-major
+// 8x8
+void kernel_sgemm_nt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 4x8
+void kernel_sgemm_nt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_sgemm_nt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+
+// A panel-major bs=8; B, C, D column-major
+// 24x4
+void kernel_sgemm_nn_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 16x4
+void kernel_sgemm_nn_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 8x8
+void kernel_sgemm_nn_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 8x4
+void kernel_sgemm_nn_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+
+// A panel-major bs=4; B, C, D column-major
+// 8x8
+void kernel_sgemm_nn_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+// 8x4
+void kernel_sgemm_nn_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+// 4x4
+void kernel_sgemm_nn_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_strsm_nn_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nn_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde);
+void kernel_strsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1);
+void kernel_strsm_nt_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nt_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde);
+void kernel_strsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1);
+void kernel_strsm_nn_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nn_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde);
+void kernel_strsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1);
+void kernel_strsm_nt_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE);
+void kernel_strsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nt_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde);
+void kernel_strsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1);
+
+// A, C, D panel-major; B, E column-major
+// TODO merge with above
+// 4x4
+void kernel_strsm_nn_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE);
+void kernel_strsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nn_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde);
+void kernel_strsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1);
+void kernel_strsm_nn_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE);
+void kernel_strsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nn_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde);
+void kernel_strsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1);
+void kernel_strsm_nt_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE);
+void kernel_strsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nt_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde);
+void kernel_strsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1);
+void kernel_strsm_nt_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE);
+void kernel_strsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1);
+void kernel_strsm_nt_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde);
+void kernel_strsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1);
+
+// A, B, C, D column-major
+void kernel_sgemm_nn_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nn_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_nt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_nt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+void kernel_sgemm_tt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd);
+void kernel_sgemm_tt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
+
+// vector
+void kernel_sdot_11_lib(int n, float *x, float *y, float *res);
+void kernel_saxpy_11_lib(int n, float *alpha, float *x, float *y);
+
+
+//#endif // BLAS_API
+
+
+
+// larger kernels
+// 24
+void kernel_sgemm_nt_24xn_p0_lib88cc(int n, int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, float *A_p, float *B_p);
+
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // BLASFEO_S_KERNEL_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h b/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h
new file mode 100644
index 000000000..9bd248b1d
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h
@@ -0,0 +1,62 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_STDLIB_H_
+#define BLASFEO_STDLIB_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+#include <stdlib.h>
+
+//
+void blasfeo_malloc(void **ptr, size_t size);
+//
+void blasfeo_malloc_align(void **ptr, size_t size);
+//
+void blasfeo_free(void *ptr);
+//
+void blasfeo_free_align(void *ptr);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // BLASFEO_STDLIB_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_target.h b/third_party/acados/include/blasfeo/include/blasfeo_target.h
new file mode 100644
index 000000000..51f617a64
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_target.h
@@ -0,0 +1,73 @@
+#ifndef TARGET_X64_INTEL_HASWELL
+#define TARGET_X64_INTEL_HASWELL
+#endif
+
+#ifndef TARGET_NEED_FEATURE_AVX2
+#define TARGET_NEED_FEATURE_AVX2 1
+#endif
+
+#ifndef TARGET_NEED_FEATURE_FMA
+#define TARGET_NEED_FEATURE_FMA 1
+#endif
+
+#ifndef TARGET_NEED_FEATURE_SSE3
+/* #undef TARGET_NEED_FEATURE_SSE3 */
+#endif
+
+#ifndef TARGET_NEED_FEATURE_AVX
+/* #undef TARGET_NEED_FEATURE_AVX */
+#endif
+
+#ifndef TARGET_NEED_FEATURE_VFPv3
+/* #undef TARGET_NEED_FEATURE_VFPv3 */
+#endif
+
+#ifndef TARGET_NEED_FEATURE_NEON
+/* #undef TARGET_NEED_FEATURE_NEON */
+#endif
+
+#ifndef TARGET_NEED_FEATURE_VFPv4
+/* #undef TARGET_NEED_FEATURE_VFPv4 */
+#endif
+
+#ifndef TARGET_NEED_FEATURE_NEONv2
+/* #undef TARGET_NEED_FEATURE_NEONv2 */
+#endif
+
+#ifndef LA_HIGH_PERFORMANCE
+#define LA_HIGH_PERFORMANCE
+#endif
+
+#ifndef MF_PANELMAJ
+#define MF_PANELMAJ
+#endif
+
+#ifndef EXT_DEP
+#define ON 1
+#define OFF 0
+#if ON==ON
+#define EXT_DEP
+#endif
+#undef ON
+#undef OFF
+#endif
+
+#ifndef BLAS_API
+#define ON 1
+#define OFF 0
+#if OFF==ON
+#define BLAS_API
+#endif
+#undef ON
+#undef OFF
+#endif
+
+#ifndef FORTRAN_BLAS_API
+#define ON 1
+#define OFF 0
+#if OFF==ON
+#define FORTRAN_BLAS_API
+#endif
+#undef ON
+#undef OFF
+#endif
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_timing.h b/third_party/acados/include/blasfeo/include/blasfeo_timing.h
new file mode 100644
index 000000000..5671b888f
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_timing.h
@@ -0,0 +1,114 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_TIMING_H_
+#define BLASFEO_TIMING_H_
+
+//#include <stdbool.h>
+
+#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__)
+
+	/* Use Windows QueryPerformanceCounter for timing. */
+	#include <Windows.h>
+
+	/** A structure for keeping internal timer data. */
+	typedef struct blasfeo_timer_ {
+		LARGE_INTEGER tic;
+		LARGE_INTEGER toc;
+		LARGE_INTEGER freq;
+	} blasfeo_timer;
+
+#elif(defined __APPLE__)
+
+	#include <mach/mach_time.h>
+
+	/** A structure for keeping internal timer data. */
+	typedef struct blasfeo_timer_ {
+		uint64_t tic;
+		uint64_t toc;
+		mach_timebase_info_data_t tinfo;
+	} blasfeo_timer;
+
+#elif(defined __DSPACE__)
+
+	#include <brtenv.h>
+
+	typedef struct blasfeo_timer_ {
+		double time;
+	} blasfeo_timer;
+
+#elif(defined __XILINX_NONE_ELF__ || defined __XILINX_ULTRASCALE_NONE_ELF_JAILHOUSE__)
+
+	#include "xtime_l.h"
+
+	typedef struct blasfeo_timer_ {
+		uint64_t tic;
+		uint64_t toc;
+	} blasfeo_timer;
+
+#else
+
+	/* Use POSIX clock_gettime() for timing on non-Windows machines. */
+	#include <time.h>
+
+	#if __STDC_VERSION__ >= 199901L  // C99 Mode
+
+		#include <sys/stat.h>
+		#include <sys/time.h>
+
+		typedef struct blasfeo_timer_ {
+			struct timeval tic;
+			struct timeval toc;
+		} blasfeo_timer;
+
+	#else  // ANSI C Mode
+
+		/** A structure for keeping internal timer data. */
+		typedef struct blasfeo_timer_ {
+			struct timespec tic;
+			struct timespec toc;
+		} blasfeo_timer;
+
+	#endif  // __STDC_VERSION__ >= 199901L
+
+#endif  // (defined _WIN32 || defined _WIN64)
+
+/** A function for measurement of the current time. */
+void blasfeo_tic(blasfeo_timer* t);
+
+/** A function which returns the elapsed time. */
+double blasfeo_toc(blasfeo_timer* t);
+
+#endif  // BLASFEO_TIMING_H_
diff --git a/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h
new file mode 100644
index 000000000..159855118
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef BLASFEO_V_AUX_EXT_DEP_H_
+#define BLASFEO_V_AUX_EXT_DEP_H_
+
+
+
+#include "blasfeo_target.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+/************************************************
+* d_aux_extern_depend_lib.c
+************************************************/
+
+#ifdef EXT_DEP
+
+void v_zeros(void **ptrA, int size);
+// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to void; set allocated memory to zero
+void v_zeros_align(void **ptrA, int size);
+// free the memory allocated by v_zeros
+void v_free(void *ptrA);
+// free the memory allocated by v_zeros_aligned
+void v_free_align(void *ptrA);
+// dynamically allocate size bytes of memory and set accordingly a pointer to char; set allocated memory to zero
+void c_zeros(char **ptrA, int size);
+// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to char; set allocated memory to zero
+void c_zeros_align(char **ptrA, int size);
+// free the memory allocated by c_zeros
+void c_free(char *ptrA);
+// free the memory allocated by c_zeros_aligned
+void c_free_align(char *ptrA);
+
+#endif // EXT_DEP
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+#endif  // BLASFEO_V_AUX_EXT_DEP_H_
diff --git a/third_party/acados/include/blasfeo/include/d_blas.h b/third_party/acados/include/blasfeo/include/d_blas.h
new file mode 100644
index 000000000..d6f878672
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/d_blas.h
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// headers to reference BLAS and LAPACK routines employed in BLASFEO WR
+
+// level 1
+double ddot_(int *m, double *x, int *incx, double *y, int *incy);
+void dcopy_(int *m, double *x, int *incx, double *y, int *incy);
+void daxpy_(int *m, double *alpha, double *x, int *incx, double *y, int *incy);
+void dscal_(int *m, double *alpha, double *x, int *incx);
+void drot_(int *m, double *x, int *incx, double *y, int *incy, double *c, double *s);
+void drotg_(double *a, double *b, double *c, double *s);
+
+// level 2
+void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
+void dsymv_(char *uplo, int *m, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
+void dtrmv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx);
+void dtrsv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx);
+void dger_(int *m, int *n, double *alpha, double *x, int *incx, double *y, int *incy, double *A, int *lda);
+
+// level 3
+void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc);
+void dsyrk_(char *uplo, char *trans, int *n, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc);
+void dtrmm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+void dtrsm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb);
+
+// lapack
+void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info);
+void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info);
+void dgeqrf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info);
+void dgeqr2_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *info);
+void dgelqf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info);
+void dorglq_(int *m, int *n, int *k, double *A, int *lda, double *tau, double *work, int *lwork, int *info);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/third_party/acados/include/blasfeo/include/d_blas_64.h b/third_party/acados/include/blasfeo/include/d_blas_64.h
new file mode 100644
index 000000000..4f40d00df
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/d_blas_64.h
@@ -0,0 +1,73 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// headers to reference BLAS and LAPACK routines employed in BLASFEO WR
+
+// level 1
+double ddot_(long long *m, double *x, long long *incx, double *y, long long *incy);
+void dcopy_(long long *m, double *x, long long *incx, double *y, long long *incy);
+void daxpy_(long long *m, double *alpha, double *x, long long *incx, double *y, long long *incy);
+void dscal_(long long *m, double *alpha, double *x, long long *incx);
+
+// level 2
+void dgemv_(char *ta, long long *m, long long *n, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy);
+void dsymv_(char *uplo, long long *m, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy);
+void dtrmv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx);
+void dtrsv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx);
+void dger_(long long *m, long long *n, double *alpha, double *x, long long *incx, double *y, long long *incy, double *A, long long *lda);
+
+// level 3
+void dgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, double *alpha, double *A, long long *lda, double *B, long long *ldb, double *beta, double *C, long long *ldc);
+void dsyrk_(char *uplo, char *trans, long long *n, long long *k, double *alpha, double *A, long long *lda, double *beta, double *C, long long *ldc);
+void dtrmm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb);
+void dtrsm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb);
+
+// lapack
+void dpotrf_(char *uplo, long long *m, double *A, long long *lda, long long *info);
+void dgetrf_(long long *m, long long *n, double *A, long long *lda, long long *ipiv, long long *info);
+void dgeqrf_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *lwork, long long *info);
+void dgeqr2_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *info);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/third_party/acados/include/blasfeo/include/s_blas.h b/third_party/acados/include/blasfeo/include/s_blas.h
new file mode 100644
index 000000000..58b2bb0a1
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/s_blas.h
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// headers to reference BLAS and LAPACK routines employed in BLASFEO WR
+
+// level 1
+float sdot_(int *m, float *x, int *incx, float *y, int *incy);
+void scopy_(int *m, float *x, int *incx, float *y, int *incy);
+void saxpy_(int *m, float *alpha, float *x, int *incx, float *y, int *incy);
+void sscal_(int *m, float *alpha, float *x, int *incx);
+void srot_(int *m, float *x, int *incx, float *y, int *incy, float *c, float *s);
+void srotg_(float *a, float *b, float *c, float *s);
+
+// level 2
+void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy);
+void ssymv_(char *uplo, int *m, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy);
+void strmv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx);
+void strsv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx);
+void sger_(int *m, int *n, float *alpha, float *x, int *incx, float *y, int *incy, float *A, int *lda);
+
+// level 3
+void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc);
+void ssyrk_(char *uplo, char *trans, int *n, int *k, float *alpha, float *A, int *lda, float *beta, float *C, int *ldc);
+void strmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb);
+void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb);
+
+// lapack
+void spotrf_(char *uplo, int *m, float *A, int *lda, int *info);
+void sgetrf_(int *m, int *n, float *A, int *lda, int *ipiv, int *info);
+void sgeqrf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info);
+void sgeqr2_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *info);
+void sgelqf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info);
+void sorglq_(int *m, int *n, int *k, float *A, int *lda, float *tau, float *work, int *lwork, int *info);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/third_party/acados/include/blasfeo/include/s_blas_64.h b/third_party/acados/include/blasfeo/include/s_blas_64.h
new file mode 100644
index 000000000..b9efab6c2
--- /dev/null
+++ b/third_party/acados/include/blasfeo/include/s_blas_64.h
@@ -0,0 +1,73 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of BLASFEO.                                                                   *
+*                                                                                                 *
+* BLASFEO -- BLAS For Embedded Optimization.                                                      *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// headers to reference BLAS and LAPACK routines employed in BLASFEO WR
+
+// level 1
+float sdot_(long long *m, float *x, long long *incx, float *y, long long *incy);
+void scopy_(long long *m, float *x, long long *incx, float *y, long long *incy);
+void saxpy_(long long *m, float *alpha, float *x, long long *incx, float *y, long long *incy);
+void sscal_(long long *m, float *alpha, float *x, long long *incx);
+
+// level 2
+void sgemv_(char *ta, long long *m, long long *n, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy);
+void ssymv_(char *uplo, long long *m, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy);
+void strmv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx);
+void strsv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx);
+void sger_(long long *m, long long *n, float *alpha, float *x, long long *incx, float *y, long long *incy, float *A, long long *lda);
+
+// level 3
+void sgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, float *alpha, float *A, long long *lda, float *B, long long *ldb, float *beta, float *C, long long *ldc);
+void ssyrk_(char *uplo, char *trans, long long *n, long long *k, float *alpha, float *A, long long *lda, float *beta, float *C, long long *ldc);
+void strmm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb);
+void strsm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb);
+
+// lapack
+void spotrf_(char *uplo, long long *m, float *A, long long *lda, long long *info);
+void sgetrf_(long long *m, long long *n, float *A, long long *lda, long long *ipiv, long long *info);
+void sgeqrf_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *lwork, long long *info);
+void sgeqr2_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *info);
+
+
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/third_party/acados/include/hpipm/include/hpipm_aux_mem.h b/third_party/acados/include/hpipm/include/hpipm_aux_mem.h
new file mode 100644
index 000000000..7bd3d7e8b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_aux_mem.h
@@ -0,0 +1,52 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_AUX_MEM_H_
+#define HPIPM_AUX_MEM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void hpipm_zero_memset(hpipm_size_t memsize, void *mem);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_AUX_MEM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_aux_string.h b/third_party/acados/include/hpipm/include/hpipm_aux_string.h
new file mode 100644
index 000000000..804cba5dc
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_aux_string.h
@@ -0,0 +1,50 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_AUX_STRING_H_
+#define HPIPM_AUX_STRING_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MAX_STR_LEN 5
+int hpipm_strcmp(char *str1, char *str2);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_AUX_STRING_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_common.h b/third_party/acados/include/hpipm/include/hpipm_common.h
new file mode 100644
index 000000000..0cc96a7b5
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_common.h
@@ -0,0 +1,76 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_COMMON_H_
+#define HPIPM_COMMON_H_
+
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef size_t hpipm_size_t;
+
+enum hpipm_mode
+	{
+	SPEED_ABS, // focus on speed, absolute IPM formulation
+	SPEED, // focus on speed, relative IPM formulation
+	BALANCE, // balanced mode, relative IPM formulation
+	ROBUST, // focus on robustness, relative IPM formulation
+	};
+
+enum hpipm_status
+	{
+	SUCCESS, // found solution satisfying accuracy tolerance
+	MAX_ITER, // maximum iteration number reached
+	MIN_STEP, // minimum step length reached
+	NAN_SOL, // NaN in solution detected
+	INCONS_EQ, // unconsistent equality constraints
+	};
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_COMMON_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h
new file mode 100644
index 000000000..0e4c41f22
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h
@@ -0,0 +1,71 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_CAST_QCQP_H_
+#define HPIPM_D_CAST_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qcqp.h"
+#include "hpipm_d_dense_qcqp_sol.h"
+#include "hpipm_d_ocp_qcqp.h"
+#include "hpipm_d_ocp_qcqp_dim.h"
+#include "hpipm_d_ocp_qcqp_sol.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_cast_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim);
+//
+void d_cast_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_CAST_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond.h b/third_party/acados/include/hpipm/include/hpipm_d_cond.h
new file mode 100644
index 000000000..5900a2ab1
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_cond.h
@@ -0,0 +1,135 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_COND_H_
+#define HPIPM_D_COND_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp.h"
+#include "hpipm_d_dense_qp_sol.h"
+#include "hpipm_d_ocp_qp.h"
+#include "hpipm_d_ocp_qp_dim.h"
+#include "hpipm_d_ocp_qp_sol.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_cond_qp_arg
+	{
+	int cond_last_stage; // condense last stage
+	int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution inequality constr (lam t)
+	int square_root_alg; // square root algorithm (faster but requires RSQ>0)
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_cond_qp_ws
+	{
+	struct blasfeo_dmat *Gamma;
+	struct blasfeo_dmat *GammaQ;
+	struct blasfeo_dmat *L;
+	struct blasfeo_dmat *Lx;
+	struct blasfeo_dmat *AL;
+	struct blasfeo_dvec *Gammab;
+	struct blasfeo_dvec *l;
+	struct blasfeo_dvec *tmp_nbgM;
+	struct blasfeo_dvec *tmp_nuxM;
+	int bs; // block size
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_cond_qp_arg_memsize();
+//
+void d_cond_qp_arg_create(struct d_cond_qp_arg *cond_arg, void *mem);
+//
+void d_cond_qp_arg_set_default(struct d_cond_qp_arg *cond_arg);
+// condensing algorithm: 0 N2-nx3, 1 N3-nx2
+void d_cond_qp_arg_set_cond_alg(int cond_alg, struct d_cond_qp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 square-root
+void d_cond_qp_arg_set_ric_alg(int ric_alg, struct d_cond_qp_arg *cond_arg);
+// condense last stage: 0 last stage disregarded, 1 last stage condensed too
+void d_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qp_arg *cond_arg);
+//
+void d_cond_qp_arg_set_comp_prim_sol(int value, struct d_cond_qp_arg *cond_arg);
+//
+void d_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_cond_qp_arg *cond_arg);
+//
+void d_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_cond_qp_arg *cond_arg);
+
+//
+void d_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, struct d_dense_qp_dim *dense_dim);
+//
+hpipm_size_t d_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg);
+//
+void d_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws, void *mem);
+//
+void d_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+// TODO remove
+void d_cond_qp_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+
+//
+void d_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_COND_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h b/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h
new file mode 100644
index 000000000..73afba3c7
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h
@@ -0,0 +1,92 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_COND_AUX_H_
+#define HPIPM_D_COND_AUX_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_cond_BAbt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_BAt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_b(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_RSQrq(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_RSQ(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_rq(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_DCt(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_cond_d(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+
+//
+void d_update_cond_BAbt(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_update_cond_RSQrq_N2nx3(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+//
+void d_update_cond_DCtd(int *idxc, struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, int *idxs2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_COND_AUX_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h
new file mode 100644
index 000000000..266567bb8
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h
@@ -0,0 +1,129 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_COND_QCQP_H_
+#define HPIPM_D_COND_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qcqp.h"
+#include "hpipm_d_dense_qcqp_sol.h"
+#include "hpipm_d_ocp_qcqp.h"
+#include "hpipm_d_ocp_qcqp_dim.h"
+#include "hpipm_d_ocp_qcqp_sol.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_cond_qcqp_arg
+	{
+	struct d_cond_qp_arg *qp_arg;
+	int cond_last_stage; // condense last stage
+//	int cond_variant; // TODO
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution equality constr (lam t)
+	int square_root_alg; // square root algorithm (faster but requires RSQ>0)
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_cond_qcqp_ws
+	{
+	struct d_cond_qp_ws *qp_ws;
+	struct blasfeo_dmat *hess_array; // TODO remove
+	struct blasfeo_dmat *zero_hess; // TODO remove
+	struct blasfeo_dvec *grad_array; // TODO remove
+	struct blasfeo_dvec *zero_grad; // TODO remove
+	struct blasfeo_dvec *tmp_nvc;
+	struct blasfeo_dvec *tmp_nuxM;
+	struct blasfeo_dmat *GammaQ;
+	struct blasfeo_dmat *tmp_DCt;
+	struct blasfeo_dmat *tmp_nuM_nxM;
+//	struct blasfeo_dvec *d_qp;
+//	struct blasfeo_dvec *d_mask_qp;
+	hpipm_size_t memsize;
+	};
+
+
+//
+hpipm_size_t d_cond_qcqp_arg_memsize();
+//
+void d_cond_qcqp_arg_create(struct d_cond_qcqp_arg *cond_arg, void *mem);
+//
+void d_cond_qcqp_arg_set_default(struct d_cond_qcqp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 square-root
+void d_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_cond_qcqp_arg *cond_arg);
+// condense last stage: 0 last stage disregarded, 1 last stage condensed too
+void d_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qcqp_arg *cond_arg);
+
+//
+void d_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim);
+//
+hpipm_size_t d_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg);
+//
+void d_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws, void *mem);
+//
+void d_cond_qcqp_qc(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_qc_lhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_qc_rhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+//
+void d_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp_sol *dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_COND_QCQP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h
new file mode 100644
index 000000000..f39d9a9b5
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h
@@ -0,0 +1,101 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_CORE_QP_IPM_
+#define HPIPM_D_CORE_QP_IPM_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct d_core_qp_ipm_workspace
+	{
+	double *v; // primal variables
+	double *pi; // equality constraints multipliers
+	double *lam; // inequality constraints multipliers
+	double *t; // inequality constraints slacks
+	double *t_inv; // inverse of t
+	double *v_bkp; // backup of primal variables
+	double *pi_bkp; // backup of equality constraints multipliers
+	double *lam_bkp; // backup of inequality constraints multipliers
+	double *t_bkp; // backup of inequality constraints slacks
+	double *dv; // step in v
+	double *dpi; // step in pi
+	double *dlam; // step in lam
+	double *dt; // step in t
+	double *res_g; // q-residuals
+	double *res_b; // b-residuals
+	double *res_d; // d-residuals
+	double *res_m; // m-residuals
+	double *res_m_bkp; // m-residuals
+	double *Gamma; // Hessian update
+	double *gamma; // gradient update
+	double alpha; // step length
+	double alpha_prim; // step length
+	double alpha_dual; // step length
+	double sigma; // centering XXX
+	double mu; // duality measuere
+	double mu_aff; // affine duality measuere
+	double nc_inv; // 1.0/nc, where nc is the total number of inequality constraints
+	double nc_mask_inv; // 1.0/nc_mask
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	double t_min_inv; // inverse of min value in t vector
+	double tau_min; // min value of barrier parameter
+	int nv; // number of primal variables
+	int ne; // number of equality constraints
+	int nc; // (twice the) number of (two-sided) inequality constraints
+	int nc_mask; // total number of ineq constr after masking
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam also in solution, or only in Gamma computation
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t d_memsize_core_qp_ipm(int nv, int ne, int nc);
+//
+void d_create_core_qp_ipm(int nv, int ne, int nc, struct d_core_qp_ipm_workspace *workspace, void *mem);
+//
+void d_core_qp_ipm(struct d_core_qp_ipm_workspace *workspace);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_D_CORE_QP_IPM_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h
new file mode 100644
index 000000000..30cc824ba
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_CORE_QP_IPM_AUX_
+#define HPIPM_S_CORE_QP_IPM_AUX_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+void d_compute_Gamma_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_lam_t_qp(double *res_d, double *res_m, double *dlam, double *dt, struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_alpha_qp(struct d_core_qp_ipm_workspace *rws);
+//
+void d_update_var_qp(struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_mu_aff_qp(struct d_core_qp_ipm_workspace *rws);
+//
+void d_backup_res_m(struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_centering_correction_qp(struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_centering_qp(struct d_core_qp_ipm_workspace *rws);
+//
+void d_compute_tau_min_qp(struct d_core_qp_ipm_workspace *rws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_S_CORE_QP_IPM_AUX_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h
new file mode 100644
index 000000000..3da571649
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h
@@ -0,0 +1,199 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QCQP_H_
+#define HPIPM_D_DENSE_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qcqp
+	{
+	struct d_dense_qcqp_dim *dim;
+	struct blasfeo_dmat *Hv; // hessian of cost & vector work space
+	struct blasfeo_dmat *A; // equality constraint matrix
+	struct blasfeo_dmat *Ct; // inequality constraints matrix
+	struct blasfeo_dmat *Hq; // hessians of quadratic constraints
+	struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks
+	struct blasfeo_dvec *b; // equality constraint vector
+	struct blasfeo_dvec *d; // inequality constraints vector
+	struct blasfeo_dvec *d_mask; // inequality constraints mask vector
+	struct blasfeo_dvec *m; // rhs of complementarity condition
+	struct blasfeo_dvec *Z; // (diagonal) hessian of slacks
+	int *idxb; // indices of box constrained variables within [u; x]
+	int *idxs_rev; // index of soft constraints (reverse storage)
+	int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_dense_qcqp_memsize(struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp *qp, void *memory);
+
+//
+void d_dense_qcqp_set(char *field, void *value, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_H(double *H, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_g(double *g, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_A(double *A, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_b(double *b, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_idxb(int *idxb, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_lb(double *lb, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_lb_mask(double *lb, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ub(double *ub, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ub_mask(double *ub, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_C(double *C, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_lg(double *lg, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_lg_mask(double *lg, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ug(double *ug, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ug_mask(double *ug, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_Hq(double *Hq, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_gq(double *gq, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_uq(double *uq, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_uq_mask(double *uq, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_idxs(int *idxs, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_idxs_rev(int *idxs_rev, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_Zl(double *Zl, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_Zu(double *Zu, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_zl(double *zl, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_zu(double *zu, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ls(double *ls, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_ls_mask(double *ls, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_us(double *us, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_set_us_mask(double *us, struct d_dense_qcqp *qp);
+
+// getters (COLMAJ)
+
+void d_dense_qcqp_get_H(struct d_dense_qcqp *qp, double *H);
+//
+void d_dense_qcqp_get_g(struct d_dense_qcqp *qp, double *g);
+//
+void d_dense_qcqp_get_A(struct d_dense_qcqp *qp, double *A);
+//
+void d_dense_qcqp_get_b(struct d_dense_qcqp *qp, double *b);
+//
+void d_dense_qcqp_get_idxb(struct d_dense_qcqp *qp, int *idxb);
+//
+void d_dense_qcqp_get_lb(struct d_dense_qcqp *qp, double *lb);
+//
+void d_dense_qcqp_get_lb_mask(struct d_dense_qcqp *qp, double *lb);
+//
+void d_dense_qcqp_get_ub(struct d_dense_qcqp *qp, double *ub);
+//
+void d_dense_qcqp_get_ub_mask(struct d_dense_qcqp *qp, double *ub);
+//
+void d_dense_qcqp_get_C(struct d_dense_qcqp *qp, double *C);
+//
+void d_dense_qcqp_get_lg(struct d_dense_qcqp *qp, double *lg);
+//
+void d_dense_qcqp_get_lg_mask(struct d_dense_qcqp *qp, double *lg);
+//
+void d_dense_qcqp_get_ug(struct d_dense_qcqp *qp, double *ug);
+//
+void d_dense_qcqp_get_ug_mask(struct d_dense_qcqp *qp, double *ug);
+//
+void d_dense_qcqp_get_idxs(struct d_dense_qcqp *qp, int *idxs);
+//
+void d_dense_qcqp_get_idxs_rev(struct d_dense_qcqp *qp, int *idxs_rev);
+//
+void d_dense_qcqp_get_Zl(struct d_dense_qcqp *qp, double *Zl);
+//
+void d_dense_qcqp_get_Zu(struct d_dense_qcqp *qp, double *Zu);
+//
+void d_dense_qcqp_get_zl(struct d_dense_qcqp *qp, double *zl);
+//
+void d_dense_qcqp_get_zu(struct d_dense_qcqp *qp, double *zu);
+//
+void d_dense_qcqp_get_ls(struct d_dense_qcqp *qp, double *ls);
+//
+void d_dense_qcqp_get_ls_mask(struct d_dense_qcqp *qp, double *ls);
+//
+void d_dense_qcqp_get_us(struct d_dense_qcqp *qp, double *us);
+//
+void d_dense_qcqp_get_us_mask(struct d_dense_qcqp *qp, double *us);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h
new file mode 100644
index 000000000..fa8c574a1
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h
@@ -0,0 +1,98 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QCQP_DIM_H_
+#define HPIPM_D_DENSE_QCQP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qcqp_dim
+	{
+	struct d_dense_qp_dim *qp_dim; // dim of qp approximation
+	int nv;  // number of variables
+	int ne;  // number of equality constraints
+	int nb;  // number of box constraints
+	int ng;  // number of general constraints
+	int nq;  // number of quadratic constraints
+	int nsb; // number of softened box constraints
+	int nsg; // number of softened general constraints
+	int nsq; // number of softened quadratic constraints
+	int ns;  // number of softened constraints (nsb+nsg+nsq) TODO number of slacks
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qcqp_dim_memsize();
+//
+void d_dense_qcqp_dim_create(struct d_dense_qcqp_dim *dim, void *memory);
+//
+void d_dense_qcqp_dim_set(char *field_name, int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nv(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_ne(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nb(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_ng(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nq(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nsb(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nsg(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_nsq(int value, struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_dim_set_ns(int value, struct d_dense_qcqp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QCQP_DIM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h
new file mode 100644
index 000000000..fa3f98fa7
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h
@@ -0,0 +1,193 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QCQP_IPM_H_
+#define HPIPM_D_DENSE_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_dense_qcqp_dim.h>
+#include <hpipm_d_dense_qcqp.h>
+#include <hpipm_d_dense_qcqp_res.h>
+#include <hpipm_d_dense_qcqp_sol.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qcqp_ipm_arg
+	{
+	struct d_dense_qp_ipm_arg *qp_arg;
+	double mu0; // initial value for duality measure
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double reg_dual; // reg of dual hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int scale; // scale hessian
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_dense_qcqp_ipm_ws
+	{
+	struct d_dense_qp_ipm_ws *qp_ws;
+	struct d_dense_qp *qp;
+	struct d_dense_qp_sol *qp_sol;
+	struct d_dense_qcqp_res_ws *qcqp_res_ws;
+	struct d_dense_qcqp_res *qcqp_res;
+	struct blasfeo_dvec *tmp_nv;
+	int iter; // iteration number
+	int status;
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t d_dense_qcqp_ipm_arg_memsize(struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_ipm_arg_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_ipm_arg *arg, void *mem);
+//
+void d_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set(char *field, void *value, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_mu0(double *mu0, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_lam_min(double *value, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_t_min(double *value, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_split_step(int *value, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t d_dense_qcqp_ipm_ws_memsize(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_ipm_ws_create(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws, void *mem);
+//
+void d_dense_qcqp_ipm_get(char *field, struct d_dense_qcqp_ipm_ws *ws, void *value);
+//
+void d_dense_qcqp_ipm_get_status(struct d_dense_qcqp_ipm_ws *ws, int *status);
+//
+void d_dense_qcqp_ipm_get_iter(struct d_dense_qcqp_ipm_ws *ws, int *iter);
+//
+void d_dense_qcqp_ipm_get_max_res_stat(struct d_dense_qcqp_ipm_ws *ws, double *res_stat);
+//
+void d_dense_qcqp_ipm_get_max_res_eq(struct d_dense_qcqp_ipm_ws *ws, double *res_eq);
+//
+void d_dense_qcqp_ipm_get_max_res_ineq(struct d_dense_qcqp_ipm_ws *ws, double *res_ineq);
+//
+void d_dense_qcqp_ipm_get_max_res_comp(struct d_dense_qcqp_ipm_ws *ws, double *res_comp);
+//
+void d_dense_qcqp_ipm_get_stat(struct d_dense_qcqp_ipm_ws *ws, double **stat);
+//
+void d_dense_qcqp_ipm_get_stat_m(struct d_dense_qcqp_ipm_ws *ws, int *stat_m);
+//
+void d_dense_qcqp_init_var(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws);
+//
+void d_dense_qcqp_ipm_solve(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws);
+#if 0
+//
+void d_dense_qcqp_ipm_predict(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws);
+//
+void d_dense_qcqp_ipm_sens(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws);
+#endif
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QCQP_IPM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h
new file mode 100644
index 000000000..a76f16215
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h
@@ -0,0 +1,107 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QCQP_RES_H_
+#define HPIPM_D_DENSE_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_d_dense_qcqp_dim.h>
+#include <hpipm_d_dense_qcqp.h>
+#include <hpipm_d_dense_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qcqp_res
+	{
+	struct d_dense_qcqp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // infinity norm of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_dense_qcqp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nv; // work space of size nv
+	struct blasfeo_dvec *tmp_nbgq; // work space of size nbM+ngM+nqM
+	struct blasfeo_dvec *tmp_ns; // work space of size nsM
+	struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qcqp_res_memsize(struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_res_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res *res, void *mem);
+//
+hpipm_size_t d_dense_qcqp_res_ws_memsize(struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_res_ws_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res_ws *workspace, void *mem);
+//
+void d_dense_qcqp_res_compute(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_res *res, struct d_dense_qcqp_res_ws *ws);
+//
+void d_dense_qcqp_res_compute_inf_norm(struct d_dense_qcqp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_DENSE_QCQP_RES_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h
new file mode 100644
index 000000000..6c697a8e6
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h
@@ -0,0 +1,85 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QCQP_SOL_H_
+#define HPIPM_D_DENSE_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qcqp_sol
+	{
+	struct d_dense_qcqp_dim *dim;
+	struct blasfeo_dvec *v;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	void *misc;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qcqp_sol_memsize(struct d_dense_qcqp_dim *dim);
+//
+void d_dense_qcqp_sol_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_sol *qp_sol, void *memory);
+//
+void d_dense_qcqp_sol_get_v(struct d_dense_qcqp_sol *qp_sol, double *v);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QCQP_SOL_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h
new file mode 100644
index 000000000..a34218bae
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h
@@ -0,0 +1,82 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QCQP_UTILS_H_
+#define HPIPM_D_DENSE_QCQP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qcqp_dim.h"
+#include "hpipm_d_dense_qcqp.h"
+#include "hpipm_d_dense_qcqp_sol.h"
+//#include "hpipm_d_dense_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_dense_qcqp_dim_print(struct d_dense_qcqp_dim *qp_dim);
+//
+//void d_dense_qcqp_dim_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim);
+//
+void d_dense_qcqp_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp);
+//
+//void d_dense_qcqp_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp);
+//
+void d_dense_qcqp_sol_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_sol *dense_qcqp_sol);
+//
+//void d_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg);
+//
+void d_dense_qcqp_res_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_res *dense_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QCQP_UTILS_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h
new file mode 100644
index 000000000..02fba5a92
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h
@@ -0,0 +1,207 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QP_H_
+#define HPIPM_D_DENSE_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qp
+	{
+	struct d_dense_qp_dim *dim;
+	struct blasfeo_dmat *Hv; // hessian of cost & vector work space
+	struct blasfeo_dmat *A; // equality constraint matrix
+	struct blasfeo_dmat *Ct; // inequality constraints matrix
+	struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks
+	struct blasfeo_dvec *b; // equality constraint vector
+	struct blasfeo_dvec *d; // inequality constraints vector
+	struct blasfeo_dvec *d_mask; // inequality constraints mask vector
+	struct blasfeo_dvec *m; // rhs of complementarity condition
+	struct blasfeo_dvec *Z; // (diagonal) hessian of slacks
+	int *idxb; // indices of box constrained variables within [u; x]
+	int *idxs_rev; // index of soft constraints (reverse storage)
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_dense_qp_memsize(struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_create(struct d_dense_qp_dim *dim, struct d_dense_qp *qp, void *memory);
+
+// setters - colmaj
+//
+void d_dense_qp_set_all(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp);
+//
+void d_dense_qp_get_all(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us);
+//
+void d_dense_qp_set(char *field, void *value, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_H(double *H, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_g(double *g, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_A(double *A, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_b(double *b, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_idxb(int *idxb, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_Jb(double *Jb, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_lb(double *lb, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_lb_mask(double *lb, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ub(double *ub, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ub_mask(double *ub, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_C(double *C, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_lg(double *lg, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_lg_mask(double *lg, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ug(double *ug, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ug_mask(double *ug, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_idxs(int *idxs, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_idxs_rev(int *idxs_rev, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_Jsb(double *Jsb, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_Jsg(double *Jsg, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_Zl(double *Zl, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_Zu(double *Zu, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_zl(double *zl, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_zu(double *zu, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ls(double *ls, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_ls_mask(double *ls, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_us(double *us, struct d_dense_qp *qp);
+//
+void d_dense_qp_set_us_mask(double *us, struct d_dense_qp *qp);
+
+// getters - colmaj
+//
+void d_dense_qp_get_H(struct d_dense_qp *qp, double *H);
+//
+void d_dense_qp_get_g(struct d_dense_qp *qp, double *g);
+//
+void d_dense_qp_get_A(struct d_dense_qp *qp, double *A);
+//
+void d_dense_qp_get_b(struct d_dense_qp *qp, double *b);
+//
+void d_dense_qp_get_idxb(struct d_dense_qp *qp, int *idxb);
+//
+void d_dense_qp_get_lb(struct d_dense_qp *qp, double *lb);
+//
+void d_dense_qp_get_lb_mask(struct d_dense_qp *qp, double *lb);
+//
+void d_dense_qp_get_ub(struct d_dense_qp *qp, double *ub);
+//
+void d_dense_qp_get_ub_mask(struct d_dense_qp *qp, double *ub);
+//
+void d_dense_qp_get_C(struct d_dense_qp *qp, double *C);
+//
+void d_dense_qp_get_lg(struct d_dense_qp *qp, double *lg);
+//
+void d_dense_qp_get_lg_mask(struct d_dense_qp *qp, double *lg);
+//
+void d_dense_qp_get_ug(struct d_dense_qp *qp, double *ug);
+//
+void d_dense_qp_get_ug_mask(struct d_dense_qp *qp, double *ug);
+//
+void d_dense_qp_get_idxs(struct d_dense_qp *qp, int *idxs);
+//
+void d_dense_qp_get_idxs_rev(struct d_dense_qp *qp, int *idxs_rev);
+//
+void d_dense_qp_get_Zl(struct d_dense_qp *qp, double *Zl);
+//
+void d_dense_qp_get_Zu(struct d_dense_qp *qp, double *Zu);
+//
+void d_dense_qp_get_zl(struct d_dense_qp *qp, double *zl);
+//
+void d_dense_qp_get_zu(struct d_dense_qp *qp, double *zu);
+//
+void d_dense_qp_get_ls(struct d_dense_qp *qp, double *ls);
+//
+void d_dense_qp_get_ls_mask(struct d_dense_qp *qp, double *ls);
+//
+void d_dense_qp_get_us(struct d_dense_qp *qp, double *us);
+//
+void d_dense_qp_get_us_mask(struct d_dense_qp *qp, double *us);
+
+// setters - rowmaj
+//
+void d_dense_qp_set_all_rowmaj(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp);
+
+// getters - rowmaj
+//
+void d_dense_qp_get_all_rowmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h
new file mode 100644
index 000000000..98a551f31
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h
@@ -0,0 +1,92 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QP_DIM_H_
+#define HPIPM_D_DENSE_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qp_dim
+	{
+	int nv;  // number of variables
+	int ne;  // number of equality constraints
+	int nb;  // number of box constraints
+	int ng;  // number of general constraints
+	int nsb; // number of softened box constraints
+	int nsg; // number of softened general constraints
+	int ns;  // number of softened constraints (nsb+nsg)
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qp_dim_memsize();
+//
+void d_dense_qp_dim_create(struct d_dense_qp_dim *qp_dim, void *memory);
+//
+void d_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set(char *field_name, int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_nv(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_ne(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_nb(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_ng(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_nsb(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_nsg(int value, struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_dim_set_ns(int value, struct d_dense_qp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h
new file mode 100644
index 000000000..e6e5d5d9b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h
@@ -0,0 +1,260 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QP_IPM_H_
+#define HPIPM_D_DENSE_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_dense_qp_dim.h>
+#include <hpipm_d_dense_qp.h>
+#include <hpipm_d_dense_qp_res.h>
+#include <hpipm_d_dense_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qp_ipm_arg
+	{
+	double mu0; // initial value for duality measure
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double reg_dual; // reg of dual hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	double tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int scale; // scale hessian
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1)
+	int comp_res_pred; // compute residuals of prediction
+	int kkt_fact_alg; // 0 null-space, 1 schur-complement
+	int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints
+	int compute_obj; // compute obj on exit
+	int split_step; // use different steps for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_dense_qp_ipm_ws
+	{
+	struct d_core_qp_ipm_workspace *core_workspace;
+	struct d_dense_qp_res_ws *res_ws;
+	struct d_dense_qp_sol *sol_step;
+	struct d_dense_qp_sol *sol_itref;
+	struct d_dense_qp *qp_step;
+	struct d_dense_qp *qp_itref;
+	struct d_dense_qp_res *res;
+	struct d_dense_qp_res *res_itref;
+	struct d_dense_qp_res *res_step;
+	struct blasfeo_dvec *Gamma; //
+	struct blasfeo_dvec *gamma; //
+	struct blasfeo_dvec *Zs_inv; //
+	struct blasfeo_dmat *Lv; //
+	struct blasfeo_dmat *AL; //
+	struct blasfeo_dmat *Le; //
+	struct blasfeo_dmat *Ctx; //
+	struct blasfeo_dvec *lv; //
+	struct blasfeo_dvec *sv; // scale for Lv
+	struct blasfeo_dvec *se; // scale for Le
+	struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng
+	struct blasfeo_dvec *tmp_ns; // work space of size ns
+	struct blasfeo_dmat *lq0;
+	struct blasfeo_dmat *lq1;
+	struct blasfeo_dvec *tmp_m;
+	struct blasfeo_dmat *A_LQ;
+	struct blasfeo_dmat *A_Q;
+	struct blasfeo_dmat *Zt;
+	struct blasfeo_dmat *ZtH;
+	struct blasfeo_dmat *ZtHZ;
+	struct blasfeo_dvec *xy;
+	struct blasfeo_dvec *Yxy;
+	struct blasfeo_dvec *xz;
+	struct blasfeo_dvec *tmp_nv;
+	struct blasfeo_dvec *tmp_2ns;
+	struct blasfeo_dvec *tmp_nv2ns;
+	struct blasfeo_dmat *A_li; // A of linearly independent equality constraints
+	struct blasfeo_dvec *b_li; // b of linearly independent equality constraints
+	struct blasfeo_dmat *A_bkp; // pointer to backup A
+	struct blasfeo_dvec *b_bkp; // pointer to backup b
+	struct blasfeo_dmat *Ab_LU;
+	double *stat; // convergence statistics
+	int *ipiv_v;
+	int *ipiv_e;
+	int *ipiv_e1;
+	void *lq_work0;
+	void *lq_work1;
+	void *lq_work_null;
+	void *orglq_work_null;
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // numer of recorded stat per ipm iter
+	int scale;
+	int use_hess_fact;
+	int use_A_fact;
+	int status;
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	int ne_li; // number of linearly independent equality constraints
+	int ne_bkp; // ne backup
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t d_dense_qp_ipm_arg_memsize(struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_ipm_arg_create(struct d_dense_qp_dim *dim, struct d_dense_qp_ipm_arg *arg, void *mem);
+//
+void d_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set(char *field, void *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_mu0(double *mu0, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_lam_min(double *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_t_min(double *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_tau_min(double *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_compute_obj(int *value, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qp_ipm_arg *arg);
+
+//
+void d_dense_qp_ipm_arg_set_split_step(int *value, struct d_dense_qp_ipm_arg *arg);
+
+//
+hpipm_size_t d_dense_qp_ipm_ws_memsize(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_ipm_ws_create(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws, void *mem);
+//
+void d_dense_qp_ipm_get(char *field, struct d_dense_qp_ipm_ws *ws, void *value);
+//
+void d_dense_qp_ipm_get_status(struct d_dense_qp_ipm_ws *ws, int *status);
+//
+void d_dense_qp_ipm_get_iter(struct d_dense_qp_ipm_ws *ws, int *iter);
+//
+void d_dense_qp_ipm_get_max_res_stat(struct d_dense_qp_ipm_ws *ws, double *res_stat);
+//
+void d_dense_qp_ipm_get_max_res_eq(struct d_dense_qp_ipm_ws *ws, double *res_eq);
+//
+void d_dense_qp_ipm_get_max_res_ineq(struct d_dense_qp_ipm_ws *ws, double *res_ineq);
+//
+void d_dense_qp_ipm_get_max_res_comp(struct d_dense_qp_ipm_ws *ws, double *res_comp);
+//
+void d_dense_qp_ipm_get_stat(struct d_dense_qp_ipm_ws *ws, double **stat);
+//
+void d_dense_qp_ipm_get_stat_m(struct d_dense_qp_ipm_ws *ws, int *stat_m);
+//
+void d_dense_qp_init_var(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_ipm_abs_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_ipm_delta_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_ipm_solve(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_ipm_predict(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_ipm_sens(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_compute_step_length(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h
new file mode 100644
index 000000000..6d05779f4
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h
@@ -0,0 +1,72 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QP_KKT_H_
+#define HPIPM_D_DENSE_QP_KKT_H_
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_fact_solve_kkt_unconstr_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_fact_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_fact_lq_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_remove_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_restore_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+//
+void d_dense_qp_compute_obj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_KKT_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h
new file mode 100644
index 000000000..7c2023257
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h
@@ -0,0 +1,105 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QP_RES_H_
+#define HPIPM_D_DENSE_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_d_dense_qp_dim.h>
+#include <hpipm_d_dense_qp.h>
+#include <hpipm_d_dense_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qp_res
+	{
+	struct d_dense_qp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // max of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_dense_qp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nbg; // work space of size nbM+ngM
+	struct blasfeo_dvec *tmp_ns; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qp_res_memsize(struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_res_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res *res, void *mem);
+//
+hpipm_size_t d_dense_qp_res_ws_memsize(struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_res_ws_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res_ws *workspace, void *mem);
+//
+void d_dense_qp_res_compute(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws);
+//
+void d_dense_qp_res_compute_lin(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_sol *qp_step, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws);
+//
+void d_dense_qp_res_compute_inf_norm(struct d_dense_qp_res *res);
+//
+void d_dense_qp_res_get_all(struct d_dense_qp_res *res, double *res_g, double *res_ls, double *res_us, double *res_b, double *res_d_lb, double *res_d_ub, double *res_d_lg, double *res_d_ug, double *res_d_ls, double *res_d_us, double *res_m_lb, double *res_m_ub, double *res_m_lg, double *res_m_ug, double *res_m_ls, double *res_m_us);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_DENSE_QP_RES_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h
new file mode 100644
index 000000000..aaa3fdb0e
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h
@@ -0,0 +1,94 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_DENSE_QP_SOL_H_
+#define HPIPM_D_DENSE_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_dense_qp_sol
+	{
+	struct d_dense_qp_dim *dim;
+	struct blasfeo_dvec *v;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	void *misc;
+	double obj;
+	int valid_obj;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_dense_qp_sol_memsize(struct d_dense_qp_dim *dim);
+//
+void d_dense_qp_sol_create(struct d_dense_qp_dim *dim, struct d_dense_qp_sol *qp_sol, void *memory);
+//
+void d_dense_qp_sol_get_all(struct d_dense_qp_sol *qp_sol, double *v, double *ls, double *us, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug, double *lam_ls, double *lam_us);
+//
+void d_dense_qp_sol_get(char *field, struct d_dense_qp_sol *sol, void *value);
+//
+void d_dense_qp_sol_get_v(struct d_dense_qp_sol *sol, double *v);
+//
+void d_dense_qp_sol_get_valid_obj(struct d_dense_qp_sol *sol, int *valid_obj);
+//
+void d_dense_qp_sol_get_obj(struct d_dense_qp_sol *sol, double *obj);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h
new file mode 100644
index 000000000..ccb77aaca
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_DENSE_QP_UTILS_H_
+#define HPIPM_D_DENSE_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp_dim.h"
+#include "hpipm_d_dense_qp.h"
+#include "hpipm_d_dense_qp_sol.h"
+#include "hpipm_d_dense_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_dense_qp_dim_print(struct d_dense_qp_dim *qp_dim);
+//
+//void d_dense_qp_dim_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim);
+//
+void d_dense_qp_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp);
+//
+//void d_dense_qp_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp);
+//
+void d_dense_qp_sol_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_sol *dense_qp_sol);
+//
+//void d_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg);
+//
+void d_dense_qp_res_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_res *dense_qp_res);
+//
+void d_dense_qp_arg_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *qp_ipm_arg);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_DENSE_QP_UTILS_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h
new file mode 100644
index 000000000..240c9b04b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h
@@ -0,0 +1,303 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_H_
+#define HPIPM_D_OCP_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qcqp
+	{
+	struct d_ocp_qcqp_dim *dim;
+	struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space
+	struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space
+	struct blasfeo_dmat *DCt; // inequality constraints matrix
+	struct blasfeo_dmat **Hq; // hessians of quadratic constraints
+	struct blasfeo_dvec *b; // dynamics vector
+	struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks
+	struct blasfeo_dvec *d; // inequality constraints vector
+	struct blasfeo_dvec *d_mask; // inequality constraints mask vector
+	struct blasfeo_dvec *m; // rhs of complementarity condition
+	struct blasfeo_dvec *Z; // (diagonal) hessian of slacks
+	int **idxb; // indices of box constrained variables within [u; x]
+	int **idxs_rev; // index of soft constraints (reverse storage)
+	int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qcqp_strsize();
+//
+hpipm_size_t d_ocp_qcqp_memsize(struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp *qp, void *memory);
+//
+void d_ocp_qcqp_copy_all(struct d_ocp_qcqp *qp_orig, struct d_ocp_qcqp *qp_dest);
+
+// setters
+//
+void d_ocp_qcqp_set_all_zero(struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_rhs_zero(struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set(char *field_name, int stage, void *value, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_A(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_B(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_b(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Q(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_S(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_R(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_q(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_r(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lb(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lb_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ub(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ub_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lbx(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lbx_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ubx(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ubx_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lbu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lbu_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ubu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ubu_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_idxb(int stage, int *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_idxbx(int stage, int *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jbx(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_idxbu(int stage, int *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jbu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_C(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_D(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lg(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lg_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ug(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_ug_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Qq(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Sq(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Rq(int stage, double *mat, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_qq(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_rq(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_uq(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_uq_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Zl(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Zu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_zl(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_zu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lls(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lls_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lus(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_lus_mask(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_idxs(int stage, int *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jsbu(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jsbx(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jsg(int stage, double *vec, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_set_Jsq(int stage, double *vec, struct d_ocp_qcqp *qp);
+
+// getters
+//
+void d_ocp_qcqp_get(char *field, int stage, struct d_ocp_qcqp *qp, void *value);
+//
+void d_ocp_qcqp_get_A(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_B(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_b(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_Q(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_S(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_R(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_q(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_r(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ub(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ub_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lb(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lb_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lbx(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lbx_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ubx(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ubx_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lbu(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lbu_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ubu(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ubu_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_idxb(int stage, struct d_ocp_qcqp *qp, int *vec);
+//
+//void d_ocp_qcqp_get_idxbx(int stage, struct d_ocp_qcqp *qp, int *vec);
+//
+//void d_ocp_qcqp_get_Jbx(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+//void d_ocp_qcqp_get_idxbu(int stage, struct d_ocp_qcqp *qp, int *vec);
+//
+//void d_ocp_qcqp_get_Jbu(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_C(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_D(int stage, struct d_ocp_qcqp *qp, double *mat);
+//
+void d_ocp_qcqp_get_lg(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lg_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ug(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_ug_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_Zl(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_Zu(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_zl(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_zu(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lls(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lls_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lus(int stage, struct d_ocp_qcqp *qp, double *vec);
+//
+void d_ocp_qcqp_get_lus_mask(int stage, struct d_ocp_qcqp *qp, double *vec);
+// XXX only valid if there is one slack per softed constraint !!!
+void d_ocp_qcqp_get_idxs(int stage, struct d_ocp_qcqp *qp, int *vec);
+//
+void d_ocp_qcqp_get_idxs_rev(int stage, struct d_ocp_qcqp *qp, int *vec);
+//
+//void d_ocp_qcqp_get_Jsbu(int stage, struct d_ocp_qcqp *qp, float *vec);
+//
+//void d_ocp_qcqp_get_Jsbx(int stage, struct d_ocp_qcqp *qp, float *vec);
+//
+//void d_ocp_qcqp_get_Jsg(int stage, struct d_ocp_qcqp *qp, float *vec);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h
new file mode 100644
index 000000000..268628a2b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h
@@ -0,0 +1,118 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_DIM_H_
+#define HPIPM_D_OCP_QCQP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qcqp_dim
+	{
+	struct d_ocp_qp_dim *qp_dim; // dim of qp approximation
+	int *nx; // number of states
+	int *nu; // number of inputs
+	int *nb; // number of box constraints
+	int *nbx; // number of (two-sided) state box constraints
+	int *nbu; // number of (two-sided) input box constraints
+	int *ng; // number of (two-sided) general constraints
+	int *nq; // number of (upper) quadratic constraints
+	int *ns; // number of soft constraints
+	int *nsbx; // number of (two-sided) soft state box constraints
+	int *nsbu; // number of (two-sided) soft input box constraints
+	int *nsg; // number of (two-sided) soft general constraints
+	int *nsq; // number of (upper) soft quadratic constraints
+	int N; // horizon length
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qcqp_dim_strsize();
+//
+hpipm_size_t d_ocp_qcqp_dim_memsize(int N);
+//
+void d_ocp_qcqp_dim_create(int N, struct d_ocp_qcqp_dim *qp_dim, void *memory);
+//
+void d_ocp_qcqp_dim_copy_all(struct d_ocp_qcqp_dim *dim_orig, struct d_ocp_qcqp_dim *dim_dest);
+//
+void d_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nx(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nu(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_ng(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nq(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_ns(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_dim_get(struct d_ocp_qcqp_dim *dim, char *field, int stage, int *value);
+//
+void d_ocp_qcqp_dim_get_N(struct d_ocp_qcqp_dim *dim, int *value);
+//
+void d_ocp_qcqp_dim_get_nx(struct d_ocp_qcqp_dim *dim, int stage, int *value);
+//
+void d_ocp_qcqp_dim_get_nu(struct d_ocp_qcqp_dim *dim, int stage, int *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QCQP_DIM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h
new file mode 100644
index 000000000..99f2329dc
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h
@@ -0,0 +1,190 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_IPM_H_
+#define HPIPM_D_OCP_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_ocp_qcqp_dim.h>
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qcqp_res.h>
+#include <hpipm_d_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qcqp_ipm_arg
+	{
+	struct d_ocp_qp_ipm_arg *qp_arg;
+	double mu0; // initial value for complementarity slackness
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_ocp_qcqp_ipm_ws
+	{
+	struct d_ocp_qp_ipm_ws *qp_ws;
+	struct d_ocp_qp *qp;
+	struct d_ocp_qp_sol *qp_sol;
+	struct d_ocp_qcqp_res_ws *qcqp_res_ws;
+	struct d_ocp_qcqp_res *qcqp_res;
+	struct blasfeo_dvec *tmp_nuxM;
+	int iter; // iteration number
+	int status;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qcqp_ipm_arg_strsize();
+//
+hpipm_size_t d_ocp_qcqp_ipm_arg_memsize(struct d_ocp_qcqp_dim *ocp_dim);
+//
+void d_ocp_qcqp_ipm_arg_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, void *mem);
+//
+void d_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qcqp_ipm_arg *arg);
+//
+void d_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set maximum number of iterations
+void d_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set minimum step lenght
+void d_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set initial value of barrier parameter
+void d_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void d_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void d_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void d_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void d_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set regularization of primal variables
+void d_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void d_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void d_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void d_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+void d_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// compute residuals after solution
+void d_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// compute residuals of prediction
+void d_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// min value of lam in the solution
+void d_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// min value of t in the solution
+void d_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_ocp_qcqp_ipm_arg *arg);
+// use different step for primal and dual variables
+void d_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void d_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t d_ocp_qcqp_ipm_ws_strsize();
+//
+hpipm_size_t d_ocp_qcqp_ipm_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg);
+//
+void d_ocp_qcqp_ipm_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws, void *mem);
+//
+void d_ocp_qcqp_ipm_get(char *field, struct d_ocp_qcqp_ipm_ws *ws, void *value);
+//
+void d_ocp_qcqp_ipm_get_status(struct d_ocp_qcqp_ipm_ws *ws, int *status);
+//
+void d_ocp_qcqp_ipm_get_iter(struct d_ocp_qcqp_ipm_ws *ws, int *iter);
+//
+void d_ocp_qcqp_ipm_get_max_res_stat(struct d_ocp_qcqp_ipm_ws *ws, double *res_stat);
+//
+void d_ocp_qcqp_ipm_get_max_res_eq(struct d_ocp_qcqp_ipm_ws *ws, double *res_eq);
+//
+void d_ocp_qcqp_ipm_get_max_res_ineq(struct d_ocp_qcqp_ipm_ws *ws, double *res_ineq);
+//
+void d_ocp_qcqp_ipm_get_max_res_comp(struct d_ocp_qcqp_ipm_ws *ws, double *res_comp);
+//
+void d_ocp_qcqp_ipm_get_stat(struct d_ocp_qcqp_ipm_ws *ws, double **stat);
+//
+void d_ocp_qcqp_ipm_get_stat_m(struct d_ocp_qcqp_ipm_ws *ws, int *stat_m);
+//
+void d_ocp_qcqp_init_var(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws);
+//
+void d_ocp_qcqp_ipm_solve(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_OCP_QCQP_IPM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h
new file mode 100644
index 000000000..077f13467
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h
@@ -0,0 +1,114 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_RES_H_
+#define HPIPM_D_OCP_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_ocp_qcqp_dim.h>
+#include <hpipm_d_ocp_qcqp.h>
+#include <hpipm_d_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qcqp_res
+	{
+	struct d_ocp_qcqp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // max of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_ocp_qcqp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM
+	struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qcqp_res_memsize(struct d_ocp_qcqp_dim *ocp_dim);
+//
+void d_ocp_qcqp_res_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res *res, void *mem);
+//
+hpipm_size_t d_ocp_qcqp_res_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim);
+//
+void d_ocp_qcqp_res_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res_ws *workspace, void *mem);
+//
+void d_ocp_qcqp_res_compute(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_res *res, struct d_ocp_qcqp_res_ws *ws);
+//
+void d_ocp_qcqp_res_compute_inf_norm(struct d_ocp_qcqp_res *res);
+//
+void d_ocp_qcqp_res_get_max_res_stat(struct d_ocp_qcqp_res *res, double *value);
+//
+void d_ocp_qcqp_res_get_max_res_eq(struct d_ocp_qcqp_res *res, double *value);
+//
+void d_ocp_qcqp_res_get_max_res_ineq(struct d_ocp_qcqp_res *res, double *value);
+//
+void d_ocp_qcqp_res_get_max_res_comp(struct d_ocp_qcqp_res *res, double *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_OCP_QCQP_RES_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h
new file mode 100644
index 000000000..68adbfce2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h
@@ -0,0 +1,114 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_SOL_H_
+#define HPIPM_D_OCP_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qcqp_sol
+	{
+	struct d_ocp_qcqp_dim *dim;
+	struct blasfeo_dvec *ux;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qcqp_sol_strsize();
+//
+hpipm_size_t d_ocp_qcqp_sol_memsize(struct d_ocp_qcqp_dim *dim);
+//
+void d_ocp_qcqp_sol_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp_sol *qp_sol, void *memory);
+//
+void d_ocp_qcqp_sol_copy_all(struct d_ocp_qcqp_sol *qp_sol_orig, struct d_ocp_qcqp_sol *qp_sol_dest);
+//
+void d_ocp_qcqp_sol_get(char *field, int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_u(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_x(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_sl(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_su(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_pi(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_lam_lb(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_lam_ub(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_lam_lg(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_get_lam_ug(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_ocp_qcqp_sol_set(char *field, int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol);
+//
+void d_ocp_qcqp_sol_set_u(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol);
+//
+void d_ocp_qcqp_sol_set_x(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol);
+//
+void d_ocp_qcqp_sol_set_sl(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol);
+//
+void d_ocp_qcqp_sol_set_su(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QCQP_SOL_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h
new file mode 100644
index 000000000..00248f1db
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h
@@ -0,0 +1,81 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QCQP_UTILS_H_
+#define HPIPM_D_OCP_QCQP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qcqp_dim.h"
+#include "hpipm_d_ocp_qp.h"
+#include "hpipm_d_ocp_qcqp_sol.h"
+#include "hpipm_d_ocp_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_ocp_qcqp_dim_print(struct d_ocp_qcqp_dim *qcqp_dim);
+//
+void d_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim);
+//
+void d_ocp_qcqp_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp);
+//
+void d_ocp_qcqp_sol_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_sol *ocp_qcqp_sol);
+//
+void d_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_ipm_arg *arg);
+//
+void d_ocp_qcqp_res_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_res *ocp_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QCQP_UTILS_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h
new file mode 100644
index 000000000..ca26cdba3
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h
@@ -0,0 +1,306 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_H_
+#define HPIPM_D_OCP_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp
+	{
+	struct d_ocp_qp_dim *dim;
+	struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space
+	struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space
+	struct blasfeo_dmat *DCt; // inequality constraints matrix
+	struct blasfeo_dvec *b; // dynamics vector
+	struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks
+	struct blasfeo_dvec *d; // inequality constraints vector
+	struct blasfeo_dvec *d_mask; // inequality constraints mask vector
+	struct blasfeo_dvec *m; // rhs of complementarity condition
+	struct blasfeo_dvec *Z; // (diagonal) hessian of slacks
+	int **idxb; // indices of box constrained variables within [u; x]
+	int **idxs_rev; // index of soft constraints (reverse storage)
+	int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1]
+	int *diag_H_flag; // flag the fact that Hessian is diagonal
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qp_strsize();
+//
+hpipm_size_t d_ocp_qp_memsize(struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp *qp, void *memory);
+//
+void d_ocp_qp_copy_all(struct d_ocp_qp *qp_orig, struct d_ocp_qp *qp_dest);
+
+// setters
+//
+void d_ocp_qp_set_all_zero(struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_rhs_zero(struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_all_rowmaj(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set(char *field_name, int stage, void *value, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_A(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_B(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_b(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Q(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_S(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_R(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_q(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_r(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lb(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lb_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ub(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ub_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lbx(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lbx_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ubx(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ubx_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lbu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lbu_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ubu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ubu_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxb(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxbx(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jbx(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxbu(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jbu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_C(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_D(int stage, double *mat, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lg(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lg_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ug(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_ug_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Zl(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Zu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_zl(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_zu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lls(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lls_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lus(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_lus_mask(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxs(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxs_rev(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jsbu(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jsbx(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jsg(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxe(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxbxe(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxbue(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_idxge(int stage, int *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jbxe(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jbue(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_Jge(int stage, double *vec, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_set_diag_H_flag(int stage, int *value, struct d_ocp_qp *qp);
+
+// getters
+//
+void d_ocp_qp_get(char *field, int stage, struct d_ocp_qp *qp, void *value);
+//
+void d_ocp_qp_get_A(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_B(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_b(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_Q(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_S(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_R(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_q(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_r(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ub(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ub_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lb(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lb_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lbx(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lbx_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ubx(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ubx_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lbu(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lbu_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ubu(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ubu_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_idxb(int stage, struct d_ocp_qp *qp, int *vec);
+//
+//void d_ocp_qp_get_idxbx(int stage, struct d_ocp_qp *qp, int *vec);
+//
+//void d_ocp_qp_get_Jbx(int stage, struct d_ocp_qp *qp, double *vec);
+//
+//void d_ocp_qp_get_idxbu(int stage, struct d_ocp_qp *qp, int *vec);
+//
+//void d_ocp_qp_get_Jbu(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_C(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_D(int stage, struct d_ocp_qp *qp, double *mat);
+//
+void d_ocp_qp_get_lg(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lg_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ug(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_ug_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_Zl(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_Zu(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_zl(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_zu(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lls(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lls_mask(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lus(int stage, struct d_ocp_qp *qp, double *vec);
+//
+void d_ocp_qp_get_lus_mask(int stage, struct d_ocp_qp *qp, double *vec);
+// XXX only valid if there is one slack per softed constraint !!!
+void d_ocp_qp_get_idxs(int stage, struct d_ocp_qp *qp, int *vec);
+//
+void d_ocp_qp_get_idxs_rev(int stage, struct d_ocp_qp *qp, int *vec);
+//
+//void d_ocp_qp_get_Jsbu(int stage, struct d_ocp_qp *qp, float *vec);
+//
+//void d_ocp_qp_get_Jsbx(int stage, struct d_ocp_qp *qp, float *vec);
+//
+//void d_ocp_qp_get_Jsg(int stage, struct d_ocp_qp *qp, float *vec);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h
new file mode 100644
index 000000000..dcfb801d3
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h
@@ -0,0 +1,142 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_DIM_H_
+#define HPIPM_D_OCP_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp_dim
+	{
+	int *nx; // number of states
+	int *nu; // number of inputs
+	int *nb; // number of box constraints
+	int *nbx; // number of state box constraints
+	int *nbu; // number of input box constraints
+	int *ng; // number of general constraints
+	int *ns; // number of soft constraints
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int *nbxe; // number of state box constraints which are equality
+	int *nbue; // number of input box constraints which are equality
+	int *nge; // number of general constraints which are equality
+	int N; // horizon length
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qp_dim_strsize();
+//
+hpipm_size_t d_ocp_qp_dim_memsize(int N);
+//
+void d_ocp_qp_dim_create(int N, struct d_ocp_qp_dim *qp_dim, void *memory);
+//
+void d_ocp_qp_dim_copy_all(struct d_ocp_qp_dim *dim_orig, struct d_ocp_qp_dim *dim_dest);
+//
+void d_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set(char *field, int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nx(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nu(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nbx(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nbu(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_ng(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_ns(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nsbx(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nsbu(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nsg(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nbxe(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nbue(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_set_nge(int stage, int value, struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_dim_get(struct d_ocp_qp_dim *dim, char *field, int stage, int *value);
+//
+void d_ocp_qp_dim_get_N(struct d_ocp_qp_dim *dim, int *value);
+//
+void d_ocp_qp_dim_get_nx(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nu(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nbx(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nbu(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_ng(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_ns(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nsbx(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nsbu(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nsg(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nbxe(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nbue(struct d_ocp_qp_dim *dim, int stage, int *value);
+//
+void d_ocp_qp_dim_get_nge(struct d_ocp_qp_dim *dim, int stage, int *value);
+
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h
new file mode 100644
index 000000000..d2a2833f2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h
@@ -0,0 +1,250 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_IPM_H_
+#define HPIPM_D_OCP_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_ocp_qp_dim.h>
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_res.h>
+#include <hpipm_d_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp_ipm_arg
+	{
+	double mu0; // initial value for complementarity slackness
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	double tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different steps for primal and dual variables
+	int var_init_scheme; // variables initialization scheme
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_ocp_qp_ipm_ws
+	{
+	double qp_res[4]; // infinity norm of residuals
+	struct d_core_qp_ipm_workspace *core_workspace;
+	struct d_ocp_qp_dim *dim; // cache dim
+	struct d_ocp_qp_res_ws *res_workspace;
+	struct d_ocp_qp_sol *sol_step;
+	struct d_ocp_qp_sol *sol_itref;
+	struct d_ocp_qp *qp_step;
+	struct d_ocp_qp *qp_itref;
+	struct d_ocp_qp_res *res_itref;
+	struct d_ocp_qp_res *res;
+	struct blasfeo_dvec *Gamma; // hessian update
+	struct blasfeo_dvec *gamma; // hessian update
+	struct blasfeo_dvec *tmp_nuxM; // work space of size nxM
+	struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	struct blasfeo_dvec *Pb; // Pb
+	struct blasfeo_dvec *Zs_inv;
+	struct blasfeo_dvec *tmp_m;
+	struct blasfeo_dvec *l; // cache linear part for _get_ric_xxx
+	struct blasfeo_dmat *L;
+	struct blasfeo_dmat *Ls;
+	struct blasfeo_dmat *P;
+	struct blasfeo_dmat *Lh;
+	struct blasfeo_dmat *AL;
+	struct blasfeo_dmat *lq0;
+	struct blasfeo_dmat *tmp_nxM_nxM;
+	double *stat; // convergence statistics
+	int *use_hess_fact;
+	void *lq_work0;
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // number of recorded stat per IPM iter
+	int use_Pb;
+	int status; // solver status
+	int square_root_alg; // cache from arg
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	int valid_ric_vec; // meaningful riccati vectors
+	int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qp_ipm_arg_strsize();
+//
+hpipm_size_t d_ocp_qp_ipm_arg_memsize(struct d_ocp_qp_dim *ocp_dim);
+//
+void d_ocp_qp_ipm_arg_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, void *mem);
+//
+void d_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qp_ipm_arg *arg);
+//
+void d_ocp_qp_ipm_arg_set(char *field, void *value, struct d_ocp_qp_ipm_arg *arg);
+// set maximum number of iterations
+void d_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_ocp_qp_ipm_arg *arg);
+// set minimum step lenght
+void d_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_ocp_qp_ipm_arg *arg);
+// set initial value of barrier parameter
+void d_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_ocp_qp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void d_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_ocp_qp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void d_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_ocp_qp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void d_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_ocp_qp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void d_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_ocp_qp_ipm_arg *arg);
+// set regularization of primal variables
+void d_ocp_qp_ipm_arg_set_reg_prim(double *tol_comp, struct d_ocp_qp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void d_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_ocp_qp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void d_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_ocp_qp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void d_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+void d_ocp_qp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qp_ipm_arg *arg);
+// dual solution of equality constraints (only for abs_form==1)
+void d_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_ocp_qp_ipm_arg *arg);
+// compute residuals after solution
+void d_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qp_ipm_arg *arg);
+// compute residuals of prediction
+void d_ocp_qp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qp_ipm_arg *arg);
+// min value of lam in the solution
+void d_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_ocp_qp_ipm_arg *arg);
+// min value of t in the solution
+void d_ocp_qp_ipm_arg_set_t_min(double *value, struct d_ocp_qp_ipm_arg *arg);
+// min value of tau in the solution
+void d_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_ocp_qp_ipm_arg *arg);
+// set split step: 0 same step, 1 different step for primal and dual variables
+void d_ocp_qp_ipm_arg_set_split_step(int *value, struct d_ocp_qp_ipm_arg *arg);
+// variables initialization scheme
+void d_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct d_ocp_qp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void d_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qp_ipm_arg *arg);
+
+//
+hpipm_size_t d_ocp_qp_ipm_ws_strsize();
+//
+hpipm_size_t d_ocp_qp_ipm_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg);
+//
+void d_ocp_qp_ipm_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, void *mem);
+//
+void d_ocp_qp_ipm_get(char *field, struct d_ocp_qp_ipm_ws *ws, void *value);
+//
+void d_ocp_qp_ipm_get_status(struct d_ocp_qp_ipm_ws *ws, int *status);
+//
+void d_ocp_qp_ipm_get_iter(struct d_ocp_qp_ipm_ws *ws, int *iter);
+//
+void d_ocp_qp_ipm_get_max_res_stat(struct d_ocp_qp_ipm_ws *ws, double *res_stat);
+//
+void d_ocp_qp_ipm_get_max_res_eq(struct d_ocp_qp_ipm_ws *ws, double *res_eq);
+//
+void d_ocp_qp_ipm_get_max_res_ineq(struct d_ocp_qp_ipm_ws *ws, double *res_ineq);
+//
+void d_ocp_qp_ipm_get_max_res_comp(struct d_ocp_qp_ipm_ws *ws, double *res_comp);
+//
+void d_ocp_qp_ipm_get_stat(struct d_ocp_qp_ipm_ws *ws, double **stat);
+//
+void d_ocp_qp_ipm_get_stat_m(struct d_ocp_qp_ipm_ws *ws, int *stat_m);
+//
+void d_ocp_qp_ipm_get_ric_Lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Lr);
+//
+void d_ocp_qp_ipm_get_ric_Ls(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Ls);
+//
+void d_ocp_qp_ipm_get_ric_P(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *P);
+//
+void d_ocp_qp_ipm_get_ric_lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *lr);
+//
+void d_ocp_qp_ipm_get_ric_p(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *p);
+// feedback control gain in the form u = K x + k
+void d_ocp_qp_ipm_get_ric_K(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *K);
+// feedback control gain in the form u = K x + k
+void d_ocp_qp_ipm_get_ric_k(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *k);
+//
+void d_ocp_qp_init_var(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_ipm_abs_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_ipm_delta_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_ipm_solve(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_ipm_predict(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_ipm_sens(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_OCP_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h
new file mode 100644
index 000000000..19dcec32c
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h
@@ -0,0 +1,66 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_KKT_H_
+#define HPIPM_D_OCP_QP_KKT_H_
+
+#include <hpipm_common.h>
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+//
+void d_ocp_qp_fact_solve_kkt_unconstr(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_fact_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_fact_lq_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+//
+void d_ocp_qp_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+#endif // HPIPM_D_OCP_QP_KKT_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h
new file mode 100644
index 000000000..f0482b044
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h
@@ -0,0 +1,117 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_RED_H_
+#define HPIPM_D_OCP_QP_RED_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_d_ocp_qp_dim.h>
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp_reduce_eq_dof_arg
+	{
+	double lam_min;
+	double t_min;
+	int alias_unchanged; // do not keep copy unchanged stage
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution inequality constr (lam t)
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+struct d_ocp_qp_reduce_eq_dof_ws
+	{
+	struct blasfeo_dvec *tmp_nuxM;
+	struct blasfeo_dvec *tmp_nbgM;
+	int *e_imask_ux;
+	int *e_imask_d;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+void d_ocp_qp_dim_reduce_eq_dof(struct d_ocp_qp_dim *dim, struct d_ocp_qp_dim *dim_red);
+//
+hpipm_size_t d_ocp_qp_reduce_eq_dof_arg_memsize();
+//
+void d_ocp_qp_reduce_eq_dof_arg_create(struct d_ocp_qp_reduce_eq_dof_arg *arg, void *mem);
+//
+void d_ocp_qp_reduce_eq_dof_arg_set_default(struct d_ocp_qp_reduce_eq_dof_arg *arg);
+//
+void d_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void d_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+hpipm_size_t d_ocp_qp_reduce_eq_dof_ws_memsize(struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_reduce_eq_dof_ws_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_reduce_eq_dof_ws *work, void *mem);
+//
+void d_ocp_qp_reduce_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work);
+//
+void d_ocp_qp_reduce_eq_dof_lhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work);
+//
+void d_ocp_qp_reduce_eq_dof_rhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work);
+//
+void d_ocp_qp_restore_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol_red, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_RED_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h
new file mode 100644
index 000000000..061c5729c
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h
@@ -0,0 +1,112 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_RES_H_
+#define HPIPM_D_OCP_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_ocp_qp_dim.h>
+#include <hpipm_d_ocp_qp.h>
+#include <hpipm_d_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp_res
+	{
+	struct d_ocp_qp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // max of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_ocp_qp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qp_res_memsize(struct d_ocp_qp_dim *ocp_dim);
+//
+void d_ocp_qp_res_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res *res, void *mem);
+//
+hpipm_size_t d_ocp_qp_res_ws_memsize(struct d_ocp_qp_dim *ocp_dim);
+//
+void d_ocp_qp_res_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res_ws *workspace, void *mem);
+//
+void d_ocp_qp_res_compute(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws);
+//
+void d_ocp_qp_res_compute_lin(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_sol *qp_step, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws);
+//
+void d_ocp_qp_res_compute_inf_norm(struct d_ocp_qp_res *res);
+//
+void d_ocp_qp_res_get_all(struct d_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us);
+//
+void d_ocp_qp_res_get_max_res_stat(struct d_ocp_qp_res *res, double *value);
+//
+void d_ocp_qp_res_get_max_res_eq(struct d_ocp_qp_res *res, double *value);
+//
+void d_ocp_qp_res_get_max_res_ineq(struct d_ocp_qp_res *res, double *value);
+//
+void d_ocp_qp_res_get_max_res_comp(struct d_ocp_qp_res *res, double *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_OCP_QP_RES_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h
new file mode 100644
index 000000000..eb91f3a7b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h
@@ -0,0 +1,128 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_SOL_H_
+#define HPIPM_D_OCP_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_ocp_qp_sol
+	{
+	struct d_ocp_qp_dim *dim;
+	struct blasfeo_dvec *ux;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_ocp_qp_sol_strsize();
+//
+hpipm_size_t d_ocp_qp_sol_memsize(struct d_ocp_qp_dim *dim);
+//
+void d_ocp_qp_sol_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_sol *qp_sol, void *memory);
+//
+void d_ocp_qp_sol_copy_all(struct d_ocp_qp_sol *qp_sol_orig, struct d_ocp_qp_sol *qp_sol_dest);
+//
+void d_ocp_qp_sol_get_all(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us);
+//
+void d_ocp_qp_sol_get_all_rowmaj(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us);
+//
+void d_ocp_qp_sol_set_all(double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us, struct d_ocp_qp_sol *qp_sol);
+//
+void d_ocp_qp_sol_get(char *field, int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_u(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_x(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_sl(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_su(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_pi(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_lb(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_lbu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_lbx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_ub(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_ubu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_ubx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_lg(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_get_lam_ug(int stage, struct d_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_ocp_qp_sol_set(char *field, int stage, double *vec, struct d_ocp_qp_sol *qp_sol);
+//
+void d_ocp_qp_sol_set_u(int stage, double *vec, struct d_ocp_qp_sol *qp_sol);
+//
+void d_ocp_qp_sol_set_x(int stage, double *vec, struct d_ocp_qp_sol *qp_sol);
+//
+void d_ocp_qp_sol_set_sl(int stage, double *vec, struct d_ocp_qp_sol *qp_sol);
+//
+void d_ocp_qp_sol_set_su(int stage, double *vec, struct d_ocp_qp_sol *qp_sol);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h
new file mode 100644
index 000000000..217e9d94a
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h
@@ -0,0 +1,82 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_OCP_QP_UTILS_H_
+#define HPIPM_D_OCP_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_ocp_qp_dim.h"
+#include "hpipm_d_ocp_qp.h"
+#include "hpipm_d_ocp_qp_sol.h"
+#include "hpipm_d_ocp_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_ocp_qp_dim_print(struct d_ocp_qp_dim *qp_dim);
+//
+void d_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim);
+//
+void d_ocp_qp_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp);
+//
+void d_ocp_qp_sol_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_sol *ocp_qp_sol);
+//
+void d_ocp_qp_ipm_arg_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg);
+//
+void d_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg);
+//
+void d_ocp_qp_res_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_res *ocp_qp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_UTILS_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h b/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h
new file mode 100644
index 000000000..1c5f78503
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h
@@ -0,0 +1,115 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_PART_COND_H_
+#define HPIPM_D_PART_COND_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_common.h"
+#include "hpipm_d_cond.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_part_cond_qp_arg
+	{
+	struct d_cond_qp_arg *cond_arg;
+	int N2;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_part_cond_qp_ws
+	{
+	struct d_cond_qp_ws *cond_workspace;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_part_cond_qp_arg_memsize(int N2);
+//
+void d_part_cond_qp_arg_create(int N2, struct d_part_cond_qp_arg *cond_arg, void *mem);
+//
+void d_part_cond_qp_arg_set_default(struct d_part_cond_qp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 squre-root
+void d_part_cond_qp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qp_arg *cond_arg);
+//
+void d_part_cond_qp_arg_set_comp_prim_sol(int value, struct d_part_cond_qp_arg *cond_arg);
+//
+void d_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_part_cond_qp_arg *cond_arg);
+//
+void d_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_part_cond_qp_arg *cond_arg);
+
+//
+void d_part_cond_qp_compute_block_size(int N, int N2, int *block_size);
+//
+void d_part_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim);
+//
+hpipm_size_t d_part_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg);
+//
+void d_part_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws, void *mem);
+//
+void d_part_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws);
+//
+void d_part_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws);
+//
+void d_part_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws);
+//
+void d_part_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_ocp_qp_sol *part_dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws);
+
+//
+void d_part_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_PART_COND_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h
new file mode 100644
index 000000000..2d6624e5b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h
@@ -0,0 +1,106 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_PART_COND_QCQP_H_
+#define HPIPM_D_PART_COND_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_cond_qcqp.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_part_cond_qcqp_arg
+	{
+	struct d_cond_qcqp_arg *cond_arg;
+	int N2;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_part_cond_qcqp_ws
+	{
+	struct d_cond_qcqp_ws *cond_ws;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_part_cond_qcqp_arg_memsize(int N2);
+//
+void d_part_cond_qcqp_arg_create(int N2, struct d_part_cond_qcqp_arg *cond_arg, void *mem);
+//
+void d_part_cond_qcqp_arg_set_default(struct d_part_cond_qcqp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 squre-root
+void d_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qcqp_arg *cond_arg);
+
+//
+void d_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size);
+//
+void d_part_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim);
+//
+hpipm_size_t d_part_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg);
+//
+void d_part_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws, void *mem);
+//
+void d_part_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws);
+//
+void d_part_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws);
+//
+void d_part_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws);
+//
+void d_part_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_ocp_qcqp_sol *part_dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_PART_COND_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h b/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h
new file mode 100644
index 000000000..320b66bb2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h
@@ -0,0 +1,122 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_SIM_ERK_H_
+#define HPIPM_D_SIM_ERK_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct d_sim_erk_arg
+	{
+	struct d_sim_rk_data *rk_data; // integrator data
+	double h; // step size
+	int steps; // number of steps
+//	int for_sens; // compute adjoint sensitivities
+//	int adj_sens; // compute adjoint sensitivities
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_sim_erk_ws
+	{
+	void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to ode
+	void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to forward vde
+	void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out); // function pointer to adjoint vde
+	void *ode_args; // pointer to ode args
+	struct d_sim_erk_arg *erk_arg; // erk arg
+	double *K; // internal variables
+	double *x_for; // states and forward sensitivities
+	double *x_traj; // states at all steps
+	double *l; // adjoint sensitivities
+	double *p; // parameter
+	double *x_tmp; // temporary states and forward sensitivities
+	double *adj_in;
+	double *adj_tmp;
+	int nx; // number of states
+	int np; // number of parameters
+	int nf; // number of forward sensitivities
+	int na; // number of adjoint sensitivities
+	int nf_max; // max number of forward sensitivities
+	int na_max; // max number of adjoint sensitivities
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_sim_erk_arg_memsize();
+//
+void d_sim_erk_arg_create(struct d_sim_erk_arg *erk_arg, void *mem);
+//
+void d_sim_erk_arg_set_all(struct d_sim_rk_data *rk_data, double h, int steps, struct d_sim_erk_arg *erk_arg);
+
+//
+hpipm_size_t d_sim_erk_ws_memsize(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max);
+//
+void d_sim_erk_ws_create(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct d_sim_erk_ws *work, void *memory);
+//
+void d_sim_erk_ws_set_all(int nf, int na, double *x, double *fs, double *bs, double *p, void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out), void *ode_args, struct d_sim_erk_ws *work);
+// number of directions for forward sensitivities
+void d_sim_erk_ws_set_nf(int *nf, struct d_sim_erk_ws *work);
+// parameters (e.g. inputs)
+void d_sim_erk_ws_set_p(double *p, struct d_sim_erk_ws *work);
+// state
+void d_sim_erk_ws_set_x(double *x, struct d_sim_erk_ws *work);
+// forward sensitivities
+void d_sim_erk_ws_set_fs(double *fs, struct d_sim_erk_ws *work);
+// ode funtion
+void d_sim_erk_ws_set_ode(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work);
+// forward vde function
+void d_sim_erk_ws_set_vde_for(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work);
+// ode_args, passed straight to the ode/vde_for/vde_adj functions
+void d_sim_erk_ws_set_ode_args(void *ode_args, struct d_sim_erk_ws *work);
+// state
+void d_sim_erk_ws_get_x(struct d_sim_erk_ws *work, double *x);
+// forward sensitivities
+void d_sim_erk_ws_get_fs(struct d_sim_erk_ws *work, double *fs);
+//
+void d_sim_erk_solve(struct d_sim_erk_arg *arg, struct d_sim_erk_ws *work);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_D_SIM_ERK_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h b/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h
new file mode 100644
index 000000000..a50dcbb49
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h
@@ -0,0 +1,71 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_SIM_RK_H_
+#define HPIPM_D_SIM_RK_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct d_sim_rk_data
+	{
+	double *A_rk; // A in butcher tableau
+	double *B_rk; // b in butcher tableau
+	double *C_rk; // c in butcher tableau
+	int expl; // erk vs irk
+	int ns; // number of stages
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_sim_rk_data_memsize(int ns);
+//
+void d_sim_rk_data_create(int ns, struct d_sim_rk_data *rk_data, void *memory);
+//
+void d_sim_rk_data_init_default(char *field, struct d_sim_rk_data *rk_data);
+//
+void d_sim_rk_data_set_all(int expl, double *A_rk, double *B_rk, double *C_rk, struct d_sim_rk_data *rk_data);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_D_SIM_RK_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h
new file mode 100644
index 000000000..f36b51a06
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h
@@ -0,0 +1,213 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QCQP_H_
+#define HPIPM_D_TREE_OCP_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_common.h"
+#include "hpipm_d_tree_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qcqp
+	{
+	struct d_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_dmat *BAbt; // Nn-1
+	struct blasfeo_dmat *RSQrq; // Nn
+	struct blasfeo_dmat *DCt; // Nn
+	struct blasfeo_dmat **Hq; // Nn
+	struct blasfeo_dvec *b; // Nn-1
+	struct blasfeo_dvec *rqz; // Nn
+	struct blasfeo_dvec *d; // Nn
+	struct blasfeo_dvec *d_mask; // Nn
+	struct blasfeo_dvec *m; // Nn
+	struct blasfeo_dvec *Z; // Nn
+	int **idxb; // indices of box constrained variables within [u; x] // Nn
+	int **idxs_rev; // index of soft constraints
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qcqp_memsize(struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp *qp, void *memory);
+//
+void d_tree_ocp_qcqp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_A(int edge, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_B(int edge, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_b(int edge, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Q(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_S(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_R(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_q(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_r(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lb(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ub(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lbx(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ubx(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lbu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ubu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_idxb(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jbx(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jbu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_C(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_D(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lg(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ug(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Qq(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Sq(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Rq(int node, double *mat, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_qq(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_rq(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_uq(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_uq_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Zl(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Zu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_zl(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_zu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lls(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lus(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_idxs(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jsg(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_set_Jsq(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_idxe(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_idxge(int node, int *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_Jbue(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_Jge(int node, double *vec, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qcqp *qp);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h
new file mode 100644
index 000000000..257b82b25
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h
@@ -0,0 +1,117 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QCQP_DIM_H_
+#define HPIPM_D_TREE_OCP_QCQP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qcqp_dim
+	{
+	struct d_tree_ocp_qp_dim *qp_dim; // dim of qp approximation
+	struct tree *ttree; // tree describing node conndection
+	int *nx; // number of states // Nn
+	int *nu; // number of inputs // Nn
+	int *nb; // number of box constraints // Nn
+	int *nbx; // number of state box constraints // Nn
+	int *nbu; // number of input box constraints // Nn
+	int *ng; // number of general constraints // Nn
+	int *nq; // number of (upper) quadratic constraints
+	int *ns; // number of soft constraints // Nn
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int *nsq; // number of (upper) soft quadratic constraints
+	int Nn; // number of nodes
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qcqp_dim_strsize();
+//
+hpipm_size_t d_tree_ocp_qcqp_dim_memsize(int Nn);
+//
+void d_tree_ocp_qcqp_dim_create(int Nn, struct d_tree_ocp_qcqp_dim *qp_dim, void *memory);
+//
+void d_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+//void d_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+//void d_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+//
+//void d_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct d_tree_ocp_qcqp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QCQP_DIM_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h
new file mode 100644
index 000000000..4a0fed106
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h
@@ -0,0 +1,191 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QCQP_IPM_H_
+#define HPIPM_D_TREE_OCP_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_tree_ocp_qcqp_dim.h>
+#include <hpipm_d_tree_ocp_qp.h>
+#include <hpipm_d_tree_ocp_qcqp_res.h>
+#include <hpipm_d_tree_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qcqp_ipm_arg
+	{
+	struct d_tree_ocp_qp_ipm_arg *qp_arg;
+	double mu0; // initial value for complementarity slackness
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+//	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+//	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_tree_ocp_qcqp_ipm_ws
+	{
+	struct d_tree_ocp_qp_ipm_ws *qp_ws;
+	struct d_tree_ocp_qp *qp;
+	struct d_tree_ocp_qp_sol *qp_sol;
+	struct d_tree_ocp_qcqp_res_ws *qcqp_res_ws;
+	struct d_tree_ocp_qcqp_res *qcqp_res;
+	struct blasfeo_dvec *tmp_nuxM;
+	int iter; // iteration number
+	int status;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qcqp_ipm_arg_strsize();
+//
+hpipm_size_t d_tree_ocp_qcqp_ipm_arg_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim);
+//
+void d_tree_ocp_qcqp_ipm_arg_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, void *mem);
+//
+void d_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qcqp_ipm_arg *arg);
+//
+void d_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set maximum number of iterations
+void d_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set minimum step lenght
+void d_tree_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set initial value of barrier parameter
+void d_tree_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void d_tree_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void d_tree_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void d_tree_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void d_tree_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set regularization of primal variables
+void d_tree_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void d_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void d_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void d_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+//void d_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// compute residuals after solution
+void d_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// compute residuals of prediction
+//void d_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// min value of lam in the solution
+void d_tree_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// min value of t in the solution
+void d_tree_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// use different step for primal and dual variables
+void d_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void d_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t d_tree_ocp_qcqp_ipm_ws_strsize();
+//
+hpipm_size_t d_tree_ocp_qcqp_ipm_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg);
+//
+void d_tree_ocp_qcqp_ipm_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws, void *mem);
+//
+void d_tree_ocp_qcqp_ipm_get(char *field, struct d_tree_ocp_qcqp_ipm_ws *ws, void *value);
+//
+void d_tree_ocp_qcqp_ipm_get_status(struct d_tree_ocp_qcqp_ipm_ws *ws, int *status);
+//
+void d_tree_ocp_qcqp_ipm_get_iter(struct d_tree_ocp_qcqp_ipm_ws *ws, int *iter);
+//
+void d_tree_ocp_qcqp_ipm_get_max_res_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_stat);
+//
+void d_tree_ocp_qcqp_ipm_get_max_res_eq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_eq);
+//
+void d_tree_ocp_qcqp_ipm_get_max_res_ineq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_ineq);
+//
+void d_tree_ocp_qcqp_ipm_get_max_res_comp(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_comp);
+//
+void d_tree_ocp_qcqp_ipm_get_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double **stat);
+//
+void d_tree_ocp_qcqp_ipm_get_stat_m(struct d_tree_ocp_qcqp_ipm_ws *ws, int *stat_m);
+//
+void d_tree_ocp_qcqp_init_var(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws);
+//
+void d_tree_ocp_qcqp_ipm_solve(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_TREE_OCP_QCQP_IPM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h
new file mode 100644
index 000000000..69eebb1ba
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h
@@ -0,0 +1,108 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QCQP_RES_H_
+#define HPIPM_D_TREE_OCP_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_tree_ocp_qcqp_dim.h>
+#include <hpipm_d_tree_ocp_qcqp.h>
+#include <hpipm_d_tree_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qcqp_res
+	{
+	struct d_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // max of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_tree_ocp_qcqp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM
+	struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qcqp_res_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim);
+//
+void d_tree_ocp_qcqp_res_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res *res, void *mem);
+//
+hpipm_size_t d_tree_ocp_qcqp_res_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim);
+//
+void d_tree_ocp_qcqp_res_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res_ws *ws, void *mem);
+//
+void d_tree_ocp_qcqp_res_compute(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_res *res, struct d_tree_ocp_qcqp_res_ws *ws);
+//
+void d_tree_ocp_qcqp_res_compute_inf_norm(struct d_tree_ocp_qcqp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_TREE_OCP_QCQP_RES_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h
new file mode 100644
index 000000000..cde8de5c5
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h
@@ -0,0 +1,99 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QCQP_SOL_H_
+#define HPIPM_D_TREE_OCP_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_tree_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct d_tree_ocp_qcqp_sol
+	{
+	struct d_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_dvec *ux;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qcqp_sol_memsize(struct d_tree_ocp_qcqp_dim *dim);
+//
+void d_tree_ocp_qcqp_sol_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp_sol *qp_sol, void *memory);
+//
+void d_tree_ocp_qcqp_sol_get(char *field, int node_edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_u(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_x(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_sl(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_su(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_pi(int edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_lam_lb(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_lam_ub(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_lam_lg(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qcqp_sol_get_lam_ug(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QCQP_SOL_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h
new file mode 100644
index 000000000..5ef04fcf6
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h
@@ -0,0 +1,84 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_
+#define HPIPM_D_TREE_OCP_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_tree_ocp_qcqp_dim.h"
+#include "hpipm_d_tree_ocp_qcqp.h"
+#include "hpipm_d_tree_ocp_qcqp_sol.h"
+#include "hpipm_d_tree_ocp_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_tree_ocp_qcqp_dim_print(struct d_tree_ocp_qcqp_dim *qp_dim);
+//
+//void d_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim);
+//
+void d_tree_ocp_qcqp_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp);
+//
+//void d_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp);
+//
+void d_tree_ocp_qcqp_sol_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_sol *ocp_qcqp_sol);
+//
+void d_tree_ocp_qcqp_ipm_arg_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg);
+//
+//void d_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg);
+//
+void d_tree_ocp_qcqp_res_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_res *ocp_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h
new file mode 100644
index 000000000..1666ad7c7
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h
@@ -0,0 +1,195 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_H_
+#define HPIPM_D_TREE_OCP_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_tree_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qp
+	{
+	struct d_tree_ocp_qp_dim *dim;
+	struct blasfeo_dmat *BAbt; // Nn-1
+	struct blasfeo_dmat *RSQrq; // Nn
+	struct blasfeo_dmat *DCt; // Nn
+	struct blasfeo_dvec *b; // Nn-1
+	struct blasfeo_dvec *rqz; // Nn
+	struct blasfeo_dvec *d; // Nn
+	struct blasfeo_dvec *d_mask; // Nn
+	struct blasfeo_dvec *m; // Nn
+	struct blasfeo_dvec *Z; // Nn
+	int **idxb; // indices of box constrained variables within [u; x] // Nn
+//	int **idxs; // index of soft constraints
+	int **idxs_rev; // index of soft constraints
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qp_memsize(struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp *qp, void *memory);
+//
+void d_tree_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_A(int edge, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_B(int edge, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_b(int edge, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Q(int node, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_S(int node, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_R(int node, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_q(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_r(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lb(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ub(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lbx(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ubx(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lbu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ubu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_idxb(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_idxbx(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Jbx(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_idxbu(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Jbu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_C(int node, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_D(int node, double *mat, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lg(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ug(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Zl(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Zu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_zl(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_zu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lls(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lus(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_idxs(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_set_Jsg(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_idxe(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_idxbue(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_idxge(int node, int *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_Jbue(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_Jge(int node, double *vec, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qp *qp);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h
new file mode 100644
index 000000000..659b66459
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h
@@ -0,0 +1,111 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_DIM_H_
+#define HPIPM_D_TREE_OCP_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qp_dim
+	{
+	struct tree *ttree; // tree describing node conndection
+	int *nx; // number of states // Nn
+	int *nu; // number of inputs // Nn
+	int *nb; // number of box constraints // Nn
+	int *nbx; // number of state box constraints // Nn
+	int *nbu; // number of input box constraints // Nn
+	int *ng; // number of general constraints // Nn
+	int *ns; // number of soft constraints // Nn
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int Nn; // number of nodes
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qp_dim_strsize();
+//
+hpipm_size_t d_tree_ocp_qp_dim_memsize(int Nn);
+//
+void d_tree_ocp_qp_dim_create(int Nn, struct d_tree_ocp_qp_dim *qp_dim, void *memory);
+//
+void d_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nx(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nu(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_ng(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_ns(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_dim_set_nge(int stage, int value, struct d_tree_ocp_qp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h
new file mode 100644
index 000000000..fb634208c
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h
@@ -0,0 +1,209 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_D_TREE_OCP_QP_IPM_H_
+#define HPIPM_D_TREE_OCP_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_tree_ocp_qp_dim.h>
+#include <hpipm_d_tree_ocp_qp.h>
+#include <hpipm_d_tree_ocp_qp_res.h>
+#include <hpipm_d_tree_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qp_ipm_arg
+	{
+	double mu0; // initial value for duality measure
+	double alpha_min; // exit cond on step length
+	double res_g_max; // exit cond on inf norm of residuals
+	double res_b_max; // exit cond on inf norm of residuals
+	double res_d_max; // exit cond on inf norm of residuals
+	double res_m_max; // exit cond on inf norm of residuals
+	double reg_prim; // reg of primal hessian
+	double lam_min; // min value in lam vector
+	double t_min; // min value in t vector
+	double tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1)
+	int split_step; // use different steps for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_tree_ocp_qp_ipm_ws
+	{
+	struct d_core_qp_ipm_workspace *core_workspace;
+	struct d_tree_ocp_qp_res_ws *res_workspace;
+	struct d_tree_ocp_qp_sol *sol_step;
+	struct d_tree_ocp_qp_sol *sol_itref;
+	struct d_tree_ocp_qp *qp_step;
+	struct d_tree_ocp_qp *qp_itref;
+	struct d_tree_ocp_qp_res *res_itref;
+	struct d_tree_ocp_qp_res *res;
+	struct blasfeo_dvec *Gamma; // hessian update
+	struct blasfeo_dvec *gamma; // hessian update
+	struct blasfeo_dvec *tmp_nxM; // work space of size nxM
+	struct blasfeo_dvec *tmp_nbgM; // work space of size nbgM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	struct blasfeo_dvec *Pb; // Pb
+	struct blasfeo_dvec *Zs_inv;
+	struct blasfeo_dmat *L;
+	struct blasfeo_dmat *Lh;
+	struct blasfeo_dmat *AL;
+	struct blasfeo_dmat *lq0;
+	struct blasfeo_dvec *tmp_m;
+	double *stat; // convergence statistics
+	int *use_hess_fact;
+	void *lq_work0;
+	double qp_res[4]; // infinity norm of residuals
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // number of recorded stat per IPM iter
+	int use_Pb;
+	int status; // solver status
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qp_ipm_arg_memsize(struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_ipm_arg_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, void *mem);
+//
+void d_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_reg_prim(double *reg, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qp_ipm_arg *arg);
+
+//
+hpipm_size_t d_tree_ocp_qp_ipm_ws_memsize(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_ipm_ws_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws, void *mem);
+//
+void d_tree_ocp_qp_ipm_get_status(struct d_tree_ocp_qp_ipm_ws *ws, int *status);
+//
+void d_tree_ocp_qp_ipm_get_iter(struct d_tree_ocp_qp_ipm_ws *ws, int *iter);
+//
+void d_tree_ocp_qp_ipm_get_max_res_stat(struct d_tree_ocp_qp_ipm_ws *ws, double *res_stat);
+//
+void d_tree_ocp_qp_ipm_get_max_res_eq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_eq);
+//
+void d_tree_ocp_qp_ipm_get_max_res_ineq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_ineq);
+//
+void d_tree_ocp_qp_ipm_get_max_res_comp(struct d_tree_ocp_qp_ipm_ws *ws, double *res_comp);
+//
+void d_tree_ocp_qp_ipm_get_stat(struct d_tree_ocp_qp_ipm_ws *ws, double **stat);
+//
+void d_tree_ocp_qp_ipm_get_stat_m(struct d_tree_ocp_qp_ipm_ws *ws, int *stat_m);
+//
+void d_tree_ocp_qp_init_var(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_ipm_abs_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_ipm_delta_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_ipm_solve(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h
new file mode 100644
index 000000000..4afd52fe4
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h
@@ -0,0 +1,52 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+//
+void d_tree_ocp_qp_fact_solve_kkt_unconstr(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_fact_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_fact_lq_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+//
+void d_tree_ocp_qp_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h
new file mode 100644
index 000000000..fe499080e
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h
@@ -0,0 +1,106 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_RES_H_
+#define HPIPM_D_TREE_OCP_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_d_tree_ocp_qp_dim.h>
+#include <hpipm_d_tree_ocp_qp.h>
+#include <hpipm_d_tree_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct d_tree_ocp_qp_res
+	{
+	struct d_tree_ocp_qp_dim *dim;
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	double res_max[4]; // max of residuals
+	double res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct d_tree_ocp_qp_res_ws
+	{
+	struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qp_res_memsize(struct d_tree_ocp_qp_dim *ocp_dim);
+//
+void d_tree_ocp_qp_res_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res *res, void *mem);
+//
+hpipm_size_t d_tree_ocp_qp_res_ws_memsize(struct d_tree_ocp_qp_dim *ocp_dim);
+//
+void d_tree_ocp_qp_res_ws_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res_ws *ws, void *mem);
+//
+void d_tree_ocp_qp_res_get_all(struct d_tree_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us);
+//
+void d_tree_ocp_qp_res_compute(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws);
+//
+void d_tree_ocp_qp_res_compute_lin(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_sol *qp_step, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws);
+//
+void d_tree_ocp_qp_res_compute_inf_norm(struct d_tree_ocp_qp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_TREE_OCP_QP_RES_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h
new file mode 100644
index 000000000..343d5e8ca
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h
@@ -0,0 +1,100 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_SOL_H_
+#define HPIPM_D_TREE_OCP_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_tree_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct d_tree_ocp_qp_sol
+	{
+	struct d_tree_ocp_qp_dim *dim;
+	struct blasfeo_dvec *ux;
+	struct blasfeo_dvec *pi;
+	struct blasfeo_dvec *lam;
+	struct blasfeo_dvec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t d_tree_ocp_qp_sol_memsize(struct d_tree_ocp_qp_dim *dim);
+//
+void d_tree_ocp_qp_sol_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_sol *qp_sol, void *memory);
+//
+void d_tree_ocp_qp_sol_get_all(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us);
+//
+void d_tree_ocp_qp_sol_get(char *field, int node_edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_u(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_x(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_sl(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_su(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_pi(int edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_lam_lb(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_lam_ub(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_lam_lg(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+//
+void d_tree_ocp_qp_sol_get_lam_ug(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h
new file mode 100644
index 000000000..b689fdc0f
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_
+#define HPIPM_D_TREE_OCP_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_tree_ocp_qp_dim.h"
+#include "hpipm_d_tree_ocp_qp.h"
+#include "hpipm_d_tree_ocp_qp_sol.h"
+#include "hpipm_d_tree_ocp_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void d_tree_ocp_qp_dim_print(struct d_tree_ocp_qp_dim *qp_dim);
+//
+//void d_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim);
+//
+void d_tree_ocp_qp_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp);
+//
+//void d_tree_ocp_qp_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp);
+//
+void d_tree_ocp_qp_sol_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_sol *ocp_qp_sol);
+//
+void d_tree_ocp_qp_ipm_arg_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+//void d_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg);
+//
+void d_tree_ocp_qp_res_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_res *ocp_qp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h
new file mode 100644
index 000000000..8bc101003
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+
+#ifndef HPIPM_M_DENSE_QP_H_
+#define HPIPM_M_DENSE_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp.h"
+#include "hpipm_s_dense_qp.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+void cvt_d2s_dense_qp(struct d_dense_qp *qpd, struct s_dense_qp *qps);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_M_DENSE_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h
new file mode 100644
index 000000000..4610321b9
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+
+#ifndef HPIPM_M_DENSE_QP_DIM_H_
+#define HPIPM_M_DENSE_QP_DIM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_d_dense_qp_dim.h"
+#include "hpipm_s_dense_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+void cvt_d2s_dense_qp_dim(struct d_dense_qp_dim *qpd, struct s_dense_qp_dim *qps);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_M_DENSE_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h
new file mode 100644
index 000000000..95c3dad1a
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h
@@ -0,0 +1,49 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h
new file mode 100644
index 000000000..1c44acee2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h
@@ -0,0 +1,115 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+struct m_ipm_hard_ocp_qp_workspace
+	{
+	struct d_ipm_hard_core_qp_workspace *core_workspace;
+	struct blasfeo_dvec *dux;
+	struct blasfeo_dvec *dpi;
+	struct blasfeo_dvec *dt_lb;
+	struct blasfeo_dvec *dt_lg;
+	struct blasfeo_svec *sdux; // XXX
+	struct blasfeo_svec *sdpi; // XXX
+	struct blasfeo_dvec *res_g; // q-residuals
+	struct blasfeo_dvec *res_b; // b-residuals
+	struct blasfeo_dvec *res_d; // d-residuals XXX remove ???
+	struct blasfeo_dvec *res_d_lb; // d-residuals
+	struct blasfeo_dvec *res_d_ub; // d-residuals
+	struct blasfeo_dvec *res_d_lg; // d-residuals
+	struct blasfeo_dvec *res_d_ug; // d-residuals
+	struct blasfeo_dvec *res_m; // m-residuals
+	struct blasfeo_dvec *res_m_lb; // m-residuals
+	struct blasfeo_dvec *res_m_ub; // m-residuals
+	struct blasfeo_dvec *res_m_lg; // m-residuals
+	struct blasfeo_dvec *res_m_ug; // m-residuals
+	struct blasfeo_svec *sres_g; // q-residuals // XXX
+	struct blasfeo_svec *sres_b; // b-residuals // XXX
+	struct blasfeo_dvec *Qx_lb; // hessian update
+	struct blasfeo_dvec *Qx_lg; // hessian update
+	struct blasfeo_dvec *qx_lb; // gradient update
+	struct blasfeo_dvec *qx_lg; // gradient update
+	struct blasfeo_svec *sQx_lb; // hessian update // XXX
+	struct blasfeo_svec *sQx_lg; // hessian update // XXX
+	struct blasfeo_svec *sqx_lb; // gradient update // XXX
+	struct blasfeo_svec *sqx_lg; // gradient update // XXX
+	struct blasfeo_dvec *tmp_nbM; // work space of size nbM
+	struct blasfeo_svec *tmp_nxM; // work space of size nxM // XXX
+	struct blasfeo_dvec *tmp_ngM; // work space of size ngM
+	struct blasfeo_svec *Pb; // Pb // XXX
+	struct blasfeo_smat *L; // XXX
+	struct blasfeo_smat *AL; // XXX
+	struct blasfeo_svec *sSx; // scaling
+	struct blasfeo_svec *sSi; // scaling inverted
+	double *stat; // convergence statistics
+	double res_mu; // mu-residual
+	int iter; // iteration number
+	int compute_Pb;
+	int scale;
+	};
+
+
+
+struct m_ipm_hard_ocp_qp_arg
+	{
+	double alpha_min; // exit cond on step length
+	double mu_max; // exit cond on duality measure
+	double mu0; // initial value for duality measure
+	int iter_max; // exit cond in iter number
+	};
+
+
+
+//
+hpipm_size_t m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg);
+//
+void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+//
+void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h
new file mode 100644
index 000000000..032fe95b1
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h
@@ -0,0 +1,45 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void m_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
+void m_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h
new file mode 100644
index 000000000..ba01ecb3b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h
@@ -0,0 +1,72 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_CAST_QCQP_H_
+#define HPIPM_S_CAST_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qcqp.h"
+#include "hpipm_s_dense_qcqp_sol.h"
+#include "hpipm_s_ocp_qcqp.h"
+#include "hpipm_s_ocp_qcqp_dim.h"
+#include "hpipm_s_ocp_qcqp_sol.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_cast_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim);
+//
+void s_cast_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_CAST_QCQP_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond.h b/third_party/acados/include/hpipm/include/hpipm_s_cond.h
new file mode 100644
index 000000000..30116798b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_cond.h
@@ -0,0 +1,137 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_COND_H_
+#define HPIPM_S_COND_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qp.h"
+#include "hpipm_s_dense_qp_sol.h"
+#include "hpipm_s_ocp_qp.h"
+#include "hpipm_s_ocp_qp_dim.h"
+#include "hpipm_s_ocp_qp_sol.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_cond_qp_arg
+	{
+	int cond_last_stage; // condense last stage
+	int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution inequality constr (lam t)
+	int square_root_alg; // square root algorithm (faster but requires RSQ>0)
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_cond_qp_ws
+	{
+	struct blasfeo_smat *Gamma;
+	struct blasfeo_smat *GammaQ;
+	struct blasfeo_smat *L;
+	struct blasfeo_smat *Lx;
+	struct blasfeo_smat *AL;
+	struct blasfeo_svec *Gammab;
+	struct blasfeo_svec *l;
+	struct blasfeo_svec *tmp_nbgM;
+	struct blasfeo_svec *tmp_nuxM;
+	int bs; // block size
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_cond_qp_arg_memsize();
+//
+void s_cond_qp_arg_create(struct s_cond_qp_arg *cond_arg, void *mem);
+//
+void s_cond_qp_arg_set_default(struct s_cond_qp_arg *cond_arg);
+// condensing algorithm: 0 N2-nx3, 1 N3-nx2
+void s_cond_qp_arg_set_cond_alg(int cond_alg, struct s_cond_qp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 square-root
+void s_cond_qp_arg_set_ric_alg(int ric_alg, struct s_cond_qp_arg *cond_arg);
+// condense last stage: 0 last stage disregarded, 1 last stage condensed too
+void s_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qp_arg *cond_arg);
+//
+void s_cond_qp_arg_set_comp_prim_sol(int value, struct s_cond_qp_arg *cond_arg);
+//
+void s_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_cond_qp_arg *cond_arg);
+//
+void s_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_cond_qp_arg *cond_arg);
+
+//
+void s_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, struct s_dense_qp_dim *dense_dim);
+//
+hpipm_size_t s_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg);
+//
+void s_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws, void *mem);
+//
+void s_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+// TODO remove
+void s_cond_qp_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+
+//
+void s_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_COND_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h b/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h
new file mode 100644
index 000000000..003472ab9
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h
@@ -0,0 +1,92 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_COND_AUX_H_
+#define HPIPM_S_COND_AUX_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_cond_BAbt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_BAt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_b(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_RSQrq(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_RSQ(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_rq(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_DCtd(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, int *idxs_rev2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_DCt(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, int *idxs_rev2, struct blasfeo_svec *Z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_cond_d(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+
+//
+void s_update_cond_BAbt(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_update_cond_RSQrq_N2nx3(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+//
+void s_update_cond_DCtd(int *idxc, struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, int *idxs2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_COND_AUX_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h
new file mode 100644
index 000000000..c36678abd
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h
@@ -0,0 +1,130 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_COND_QCQP_H_
+#define HPIPM_S_COND_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qcqp.h"
+#include "hpipm_s_dense_qcqp_sol.h"
+#include "hpipm_s_ocp_qcqp.h"
+#include "hpipm_s_ocp_qcqp_dim.h"
+#include "hpipm_s_ocp_qcqp_sol.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_cond_qcqp_arg
+	{
+	struct s_cond_qp_arg *qp_arg;
+	int cond_last_stage; // condense last stage
+//	int cond_variant; // TODO
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution equality constr (lam t)
+	int square_root_alg; // square root algorithm (faster but requires RSQ>0)
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_cond_qcqp_ws
+	{
+	struct s_cond_qp_ws *qp_ws;
+	struct blasfeo_smat *hess_array; // TODO remove
+	struct blasfeo_smat *zero_hess; // TODO remove
+	struct blasfeo_svec *zero_grad; // TODO remove
+	struct blasfeo_svec *grad_array; // TODO remove
+	struct blasfeo_svec *tmp_nvc;
+	struct blasfeo_svec *tmp_nuxM;
+	struct blasfeo_smat *GammaQ;
+	struct blasfeo_smat *tmp_DCt;
+	struct blasfeo_smat *tmp_nuM_nxM;
+//	struct blasfeo_svec *d_qp;
+//	struct blasfeo_svec *d_mask_qp;
+	hpipm_size_t memsize;
+	};
+
+
+//
+hpipm_size_t s_cond_qcqp_arg_memsize();
+//
+void s_cond_qcqp_arg_create(struct s_cond_qcqp_arg *cond_arg, void *mem);
+//
+void s_cond_qcqp_arg_set_default(struct s_cond_qcqp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 square-root
+void s_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_cond_qcqp_arg *cond_arg);
+// condense last stage: 0 last stage disregarded, 1 last stage condensed too
+void s_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qcqp_arg *cond_arg);
+
+//
+void s_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim);
+//
+hpipm_size_t s_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg);
+//
+void s_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws, void *mem);
+//
+void s_cond_qcqp_qc(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_qc_lhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_qc_rhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+//
+void s_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp_sol *dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_COND_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h
new file mode 100644
index 000000000..480392c7d
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h
@@ -0,0 +1,101 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_CORE_QP_IPM_
+#define HPIPM_S_CORE_QP_IPM_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct s_core_qp_ipm_workspace
+	{
+	float *v; // primal variables
+	float *pi; // equality constraints multipliers
+	float *lam; // inequality constraints multipliers
+	float *t; // inequality constraints slacks
+	float *t_inv; // inverse of t
+	float *v_bkp; // backup of primal variables
+	float *pi_bkp; // backup of equality constraints multipliers
+	float *lam_bkp; // backup of inequality constraints multipliers
+	float *t_bkp; // backup of inequality constraints slacks
+	float *dv; // step in v
+	float *dpi; // step in pi
+	float *dlam; // step in lam
+	float *dt; // step in t
+	float *res_g; // q-residuals
+	float *res_b; // b-residuals
+	float *res_d; // d-residuals
+	float *res_m; // m-residuals
+	float *res_m_bkp; // m-residuals
+	float *Gamma; // Hessian update
+	float *gamma; // gradient update
+	float alpha_prim; // step length
+	float alpha_dual; // step length
+	float alpha; // step length
+	float sigma; // centering XXX
+	float mu; // duality measuere
+	float mu_aff; // affine duality measuere
+	float nc_inv; // 1.0/nc, where nc is the total number of constraints
+	float nc_mask_inv; // 1.0/nc_mask
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	float t_min_inv; // inverse of min value in t vector
+	float tau_min; // min value of barrier parameter
+	int nv; // number of primal variables
+	int ne; // number of equality constraints
+	int nc; // (twice the) number of (two-sided) inequality constraints
+	int nc_mask; // total number of ineq constr after masking
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam also in solution, or only in Gamma computation
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t s_memsize_core_qp_ipm(int nv, int ne, int nc);
+//
+void s_create_core_qp_ipm(int nv, int ne, int nc, struct s_core_qp_ipm_workspace *workspace, void *mem);
+//
+void s_core_qp_ipm(struct s_core_qp_ipm_workspace *workspace);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_S_CORE_QP_IPM_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h
new file mode 100644
index 000000000..1ac3d7ede
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_CORE_QP_IPM_AUX_
+#define HPIPM_S_CORE_QP_IPM_AUX_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+void s_compute_Gamma_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_lam_t_qp(float *res_d, float *res_m, float *dlam, float *dt, struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_alpha_qp(struct s_core_qp_ipm_workspace *rws);
+//
+void s_update_var_qp(struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_mu_aff_qp(struct s_core_qp_ipm_workspace *rws);
+//
+void s_backup_res_m(struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_centering_correction_qp(struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_centering_qp(struct s_core_qp_ipm_workspace *rws);
+//
+void s_compute_tau_min_qp(struct s_core_qp_ipm_workspace *rws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_S_CORE_QP_IPM_AUX_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h
new file mode 100644
index 000000000..d03c06537
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h
@@ -0,0 +1,200 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QCQP_H_
+#define HPIPM_S_DENSE_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qcqp
+	{
+	struct s_dense_qcqp_dim *dim;
+	struct blasfeo_smat *Hv; // hessian of cost & vector work space
+	struct blasfeo_smat *A; // equality constraint matrix
+	struct blasfeo_smat *Ct; // inequality constraints matrix
+	struct blasfeo_smat *Hq; // hessians of quadratic constraints
+	struct blasfeo_svec *gz; // gradient of cost & gradient of slacks
+	struct blasfeo_svec *b; // equality constraint vector
+	struct blasfeo_svec *d; // inequality constraints vector
+	struct blasfeo_svec *d_mask; // inequality constraints mask vector
+	struct blasfeo_svec *m; // rhs of complementarity condition
+	struct blasfeo_svec *Z; // (diagonal) hessian of slacks
+	int *idxb; // indices of box constrained variables within [u; x]
+	int *idxs_rev; // index of soft constraints (reverse storage)
+	int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_dense_qcqp_memsize(struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp *qp, void *memory);
+
+//
+void s_dense_qcqp_set(char *field, void *value, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_H(float *H, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_g(float *g, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_A(float *A, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_b(float *b, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_idxb(int *idxb, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_lb(float *lb, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_lb_mask(float *lb, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ub(float *ub, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ub_mask(float *ub, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_C(float *C, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_lg(float *lg, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_lg_mask(float *lg, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ug(float *ug, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ug_mask(float *ug, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_Hq(float *Hq, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_gq(float *gq, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_uq(float *uq, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_uq_mask(float *uq, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_idxs(int *idxs, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_idxs_rev(int *idxs_rev, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_Zl(float *Zl, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_Zu(float *Zu, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_zl(float *zl, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_zu(float *zu, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ls(float *ls, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_ls_mask(float *ls, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_us(float *us, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_set_us_mask(float *us, struct s_dense_qcqp *qp);
+
+// getters (COLMAJ)
+
+void s_dense_qcqp_get_H(struct s_dense_qcqp *qp, float *H);
+//
+void s_dense_qcqp_get_g(struct s_dense_qcqp *qp, float *g);
+//
+void s_dense_qcqp_get_A(struct s_dense_qcqp *qp, float *A);
+//
+void s_dense_qcqp_get_b(struct s_dense_qcqp *qp, float *b);
+//
+void s_dense_qcqp_get_idxb(struct s_dense_qcqp *qp, int *idxb);
+//
+void s_dense_qcqp_get_lb(struct s_dense_qcqp *qp, float *lb);
+//
+void s_dense_qcqp_get_lb_mask(struct s_dense_qcqp *qp, float *lb);
+//
+void s_dense_qcqp_get_ub(struct s_dense_qcqp *qp, float *ub);
+//
+void s_dense_qcqp_get_ub_mask(struct s_dense_qcqp *qp, float *ub);
+//
+void s_dense_qcqp_get_C(struct s_dense_qcqp *qp, float *C);
+//
+void s_dense_qcqp_get_lg(struct s_dense_qcqp *qp, float *lg);
+//
+void s_dense_qcqp_get_lg_mask(struct s_dense_qcqp *qp, float *lg);
+//
+void s_dense_qcqp_get_ug(struct s_dense_qcqp *qp, float *ug);
+//
+void s_dense_qcqp_get_ug_mask(struct s_dense_qcqp *qp, float *ug);
+//
+void s_dense_qcqp_get_idxs(struct s_dense_qcqp *qp, int *idxs);
+//
+void s_dense_qcqp_get_idxs_rev(struct s_dense_qcqp *qp, int *idxs_rev);
+//
+void s_dense_qcqp_get_Zl(struct s_dense_qcqp *qp, float *Zl);
+//
+void s_dense_qcqp_get_Zu(struct s_dense_qcqp *qp, float *Zu);
+//
+void s_dense_qcqp_get_zl(struct s_dense_qcqp *qp, float *zl);
+//
+void s_dense_qcqp_get_zu(struct s_dense_qcqp *qp, float *zu);
+//
+void s_dense_qcqp_get_ls(struct s_dense_qcqp *qp, float *ls);
+//
+void s_dense_qcqp_get_ls_mask(struct s_dense_qcqp *qp, float *ls);
+//
+void s_dense_qcqp_get_us(struct s_dense_qcqp *qp, float *us);
+//
+void s_dense_qcqp_get_us_mask(struct s_dense_qcqp *qp, float *us);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QCQP_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h
new file mode 100644
index 000000000..04908c2c3
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h
@@ -0,0 +1,99 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QCQP_DIM_H_
+#define HPIPM_S_DENSE_QCQP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qcqp_dim
+	{
+	struct s_dense_qp_dim *qp_dim; // dim of qp approximation
+	int nv;  // number of variables
+	int ne;  // number of equality constraints
+	int nb;  // number of box constraints
+	int ng;  // number of general constraints
+	int nq;  // number of quadratic constraints
+	int nsb; // number of softened box constraints
+	int nsg; // number of softened general constraints
+	int nsq; // number of softened quadratic constraints
+	int ns;  // number of softened constraints (nsb+nsg+nsq) TODO number of slacks
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qcqp_dim_memsize();
+//
+void s_dense_qcqp_dim_create(struct s_dense_qcqp_dim *dim, void *memory);
+//
+void s_dense_qcqp_dim_set(char *fiels_name, int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nv(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_ne(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nb(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_ng(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nq(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nsb(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nsg(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_nsq(int value, struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_dim_set_ns(int value, struct s_dense_qcqp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QCQP_DIM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h
new file mode 100644
index 000000000..8f85768ee
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h
@@ -0,0 +1,204 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QCQP_IPM_H_
+#define HPIPM_S_DENSE_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_dense_qcqp_dim.h>
+#include <hpipm_s_dense_qcqp.h>
+#include <hpipm_s_dense_qcqp_res.h>
+#include <hpipm_s_dense_qcqp_sol.h>
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qcqp_ipm_arg
+	{
+	struct s_dense_qp_ipm_arg *qp_arg;
+	float mu0; // initial value for duality measure
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float reg_dual; // reg of dual hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int scale; // scale hessian
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_dense_qcqp_ipm_ws
+	{
+//	float qp_res[4]; // infinity norm of residuals
+	struct s_dense_qp_ipm_ws *qp_ws;
+	struct s_dense_qp *qp;
+	struct s_dense_qp_sol *qp_sol;
+	struct s_dense_qcqp_res_ws *qcqp_res_ws;
+	struct s_dense_qcqp_res *qcqp_res;
+	struct blasfeo_svec *tmp_nv;
+//	float *stat; // convergence statistics
+//	void *lq_work0;
+//	void *lq_work1;
+	int iter; // iteration number
+//	int stat_max; // iterations saved in stat
+//	int stat_m; // numer of recorded stat per ipm iter
+//	int scale;
+//	int use_hess_fact;
+	int status;
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t s_dense_qcqp_ipm_arg_memsize(struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_ipm_arg_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_ipm_arg *arg, void *mem);
+//
+void s_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set(char *field, void *value, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_mu0(float *mu0, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_lam_min(float *value, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_t_min(float *value, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_split_step(int *value, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t s_dense_qcqp_ipm_ws_memsize(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_ipm_ws_create(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws, void *mem);
+//
+void s_dense_qcqp_ipm_get(char *field, struct s_dense_qcqp_ipm_ws *ws, void *value);
+//
+void s_dense_qcqp_ipm_get_status(struct s_dense_qcqp_ipm_ws *ws, int *status);
+//
+void s_dense_qcqp_ipm_get_iter(struct s_dense_qcqp_ipm_ws *ws, int *iter);
+//
+void s_dense_qcqp_ipm_get_max_res_stat(struct s_dense_qcqp_ipm_ws *ws, float *res_stat);
+//
+void s_dense_qcqp_ipm_get_max_res_eq(struct s_dense_qcqp_ipm_ws *ws, float *res_eq);
+//
+void s_dense_qcqp_ipm_get_max_res_ineq(struct s_dense_qcqp_ipm_ws *ws, float *res_ineq);
+//
+void s_dense_qcqp_ipm_get_max_res_comp(struct s_dense_qcqp_ipm_ws *ws, float *res_comp);
+//
+void s_dense_qcqp_ipm_get_stat(struct s_dense_qcqp_ipm_ws *ws, float **stat);
+//
+void s_dense_qcqp_ipm_get_stat_m(struct s_dense_qcqp_ipm_ws *ws, int *stat_m);
+#if 0
+//
+void s_dense_qcqp_init_var(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws);
+#endif
+//
+void s_dense_qcqp_ipm_solve(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws);
+#if 0
+//
+void s_dense_qcqp_ipm_predict(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws);
+//
+void s_dense_qcqp_ipm_sens(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws);
+#endif
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QCQP_IPM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h
new file mode 100644
index 000000000..779658cc8
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h
@@ -0,0 +1,108 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QCQP_RES_H_
+#define HPIPM_S_DENSE_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_s_dense_qcqp_dim.h>
+#include <hpipm_s_dense_qcqp.h>
+#include <hpipm_s_dense_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qcqp_res
+	{
+	struct s_dense_qcqp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // infinity norm of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_dense_qcqp_res_ws
+	{
+	struct blasfeo_svec *tmp_nv; // work space of size nv
+	struct blasfeo_svec *tmp_nbgq; // work space of size nbM+ngM+nqM
+	struct blasfeo_svec *tmp_ns; // work space of size nsM
+	struct blasfeo_svec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_svec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qcqp_res_memsize(struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_res_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res *res, void *mem);
+//
+hpipm_size_t s_dense_qcqp_res_ws_memsize(struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_res_ws_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res_ws *workspace, void *mem);
+//
+void s_dense_qcqp_res_compute(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_res *res, struct s_dense_qcqp_res_ws *ws);
+//
+void s_dense_qcqp_res_compute_inf_norm(struct s_dense_qcqp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_DENSE_QCQP_RES_H_
+
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h
new file mode 100644
index 000000000..197a690e5
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h
@@ -0,0 +1,86 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QCQP_SOL_H_
+#define HPIPM_S_DENSE_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qcqp_sol
+	{
+	struct s_dense_qcqp_dim *dim;
+	struct blasfeo_svec *v;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	void *misc;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qcqp_sol_memsize(struct s_dense_qcqp_dim *dim);
+//
+void s_dense_qcqp_sol_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_sol *qp_sol, void *memory);
+//
+void s_dense_qcqp_sol_get_v(struct s_dense_qcqp_sol *qp_sol, float *v);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QCQP_SOL_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h
new file mode 100644
index 000000000..4f5aae26e
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QCQP_UTILS_H_
+#define HPIPM_S_DENSE_QCQP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qcqp_dim.h"
+#include "hpipm_s_dense_qcqp.h"
+#include "hpipm_s_dense_qcqp_sol.h"
+//#include "hpipm_s_dense_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_dense_qcqp_dim_print(struct s_dense_qcqp_dim *qp_dim);
+//
+//void s_dense_qcqp_dim_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim);
+//
+void s_dense_qcqp_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp);
+//
+//void s_dense_qcqp_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp);
+//
+void s_dense_qcqp_sol_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_sol *dense_qcqp_sol);
+//
+//void s_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg);
+//
+void s_dense_qcqp_res_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_res *dense_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QCQP_UTILS_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h
new file mode 100644
index 000000000..3c2517fe1
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h
@@ -0,0 +1,207 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QP_H_
+#define HPIPM_S_DENSE_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qp
+	{
+	struct s_dense_qp_dim *dim;
+	struct blasfeo_smat *Hv; // hessian & gradient
+	struct blasfeo_smat *A; // dynamics matrix
+	struct blasfeo_smat *Ct; // constraints matrix
+	struct blasfeo_svec *gz; // gradient & gradient of slacks
+	struct blasfeo_svec *b; // dynamics vector
+	struct blasfeo_svec *d; // constraints vector
+	struct blasfeo_svec *d_mask; // inequality constraints mask vector
+	struct blasfeo_svec *m; // rhs of complementarity condition
+	struct blasfeo_svec *Z; // (diagonal) hessian of slacks
+	int *idxb; // indices of box constrained variables within [u; x]
+	int *idxs_rev; // index of soft constraints (reverse storage)
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_dense_qp_memsize(struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_create(struct s_dense_qp_dim *dim, struct s_dense_qp *qp, void *memory);
+
+// setters - colmaj
+//
+void s_dense_qp_set_all(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp);
+//
+void s_dense_qp_get_all(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us);
+//
+void s_dense_qp_set(char *field, void *value, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_H(float *H, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_g(float *g, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_A(float *A, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_b(float *b, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_idxb(int *idxb, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_Jb(float *Jb, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_lb(float *lb, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_lb_mask(float *lb, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ub(float *ub, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ub_mask(float *ub, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_C(float *C, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_lg(float *lg, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_lg_mask(float *lg, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ug(float *ug, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ug_mask(float *ug, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_idxs(int *idxs, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_idxs_rev(int *idxs_rev, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_Jsb(float *Jsb, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_Jsg(float *Jsg, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_Zl(float *Zl, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_Zu(float *Zu, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_zl(float *zl, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_zu(float *zu, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ls(float *ls, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_ls_mask(float *ls, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_us(float *us, struct s_dense_qp *qp);
+//
+void s_dense_qp_set_us_mask(float *us, struct s_dense_qp *qp);
+
+// getters - colmaj
+//
+void s_dense_qp_get_H(struct s_dense_qp *qp, float *H);
+//
+void s_dense_qp_get_g(struct s_dense_qp *qp, float *g);
+//
+void s_dense_qp_get_A(struct s_dense_qp *qp, float *A);
+//
+void s_dense_qp_get_b(struct s_dense_qp *qp, float *b);
+//
+void s_dense_qp_get_idxb(struct s_dense_qp *qp, int *idxb);
+//
+void s_dense_qp_get_lb(struct s_dense_qp *qp, float *lb);
+//
+void s_dense_qp_get_lb_mask(struct s_dense_qp *qp, float *lb);
+//
+void s_dense_qp_get_ub(struct s_dense_qp *qp, float *ub);
+//
+void s_dense_qp_get_ub_mask(struct s_dense_qp *qp, float *ub);
+//
+void s_dense_qp_get_C(struct s_dense_qp *qp, float *C);
+//
+void s_dense_qp_get_lg(struct s_dense_qp *qp, float *lg);
+//
+void s_dense_qp_get_lg_mask(struct s_dense_qp *qp, float *lg);
+//
+void s_dense_qp_get_ug(struct s_dense_qp *qp, float *ug);
+//
+void s_dense_qp_get_ug_mask(struct s_dense_qp *qp, float *ug);
+//
+void s_dense_qp_get_idxs(struct s_dense_qp *qp, int *idxs);
+//
+void s_dense_qp_get_idxs_rev(struct s_dense_qp *qp, int *idxs_rev);
+//
+void s_dense_qp_get_Zl(struct s_dense_qp *qp, float *Zl);
+//
+void s_dense_qp_get_Zu(struct s_dense_qp *qp, float *Zu);
+//
+void s_dense_qp_get_zl(struct s_dense_qp *qp, float *zl);
+//
+void s_dense_qp_get_zu(struct s_dense_qp *qp, float *zu);
+//
+void s_dense_qp_get_ls(struct s_dense_qp *qp, float *ls);
+//
+void s_dense_qp_get_ls_mask(struct s_dense_qp *qp, float *ls);
+//
+void s_dense_qp_get_us(struct s_dense_qp *qp, float *us);
+//
+void s_dense_qp_get_us_mask(struct s_dense_qp *qp, float *us);
+
+// setters - rowmaj
+//
+void s_dense_qp_set_all_rowmaj(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp);
+
+// getters - rowmaj
+//
+void s_dense_qp_get_all_rowmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h
new file mode 100644
index 000000000..b979d2443
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h
@@ -0,0 +1,94 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QP_DIM_H_
+#define HPIPM_S_DENSE_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qp_dim
+	{
+	int nv;  // number of variables
+	int ne;  // number of equality constraints
+	int nb;  // number of box constraints
+	int ng;  // number of general constraints
+	int nsb; // number of softened box constraints
+	int nsg; // number of softened general constraints
+	int ns;  // number of softened constraints (nsb+nsg)
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qp_dim_memsize();
+//
+void s_dense_qp_dim_create(struct s_dense_qp_dim *qp_dim, void *memory);
+//
+void s_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set(char *fiels_name, int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_nv(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_ne(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_nb(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_ng(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_nsb(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_nsg(int value, struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_dim_set_ns(int value, struct s_dense_qp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_DIM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h
new file mode 100644
index 000000000..f2d56d452
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h
@@ -0,0 +1,260 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QP_IPM_H_
+#define HPIPM_S_DENSE_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_dense_qp_dim.h>
+#include <hpipm_s_dense_qp.h>
+#include <hpipm_s_dense_qp_res.h>
+#include <hpipm_s_dense_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qp_ipm_arg
+	{
+	float mu0; // initial value for duality measure
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float reg_dual; // reg of dual hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	float tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int scale; // scale hessian
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1)
+	int comp_res_pred; // compute residuals of prediction
+	int kkt_fact_alg; // 0 null-space, 1 schur-complement
+	int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints
+	int compute_obj; // compute obj on exit
+	int split_step; // use different steps for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_dense_qp_ipm_ws
+	{
+	struct s_core_qp_ipm_workspace *core_workspace;
+	struct s_dense_qp_res_ws *res_ws;
+	struct s_dense_qp_sol *sol_step;
+	struct s_dense_qp_sol *sol_itref;
+	struct s_dense_qp *qp_step;
+	struct s_dense_qp *qp_itref;
+	struct s_dense_qp_res *res;
+	struct s_dense_qp_res *res_itref;
+	struct s_dense_qp_res *res_step;
+	struct blasfeo_svec *Gamma; //
+	struct blasfeo_svec *gamma; //
+	struct blasfeo_svec *Zs_inv; //
+	struct blasfeo_smat *Lv; //
+	struct blasfeo_smat *AL; //
+	struct blasfeo_smat *Le; //
+	struct blasfeo_smat *Ctx; //
+	struct blasfeo_svec *lv; //
+	struct blasfeo_svec *sv; // scale for Lv
+	struct blasfeo_svec *se; // scale for Le
+	struct blasfeo_svec *tmp_nbg; // work space of size nb+ng
+	struct blasfeo_svec *tmp_ns; // work space of size ns
+	struct blasfeo_smat *lq0;
+	struct blasfeo_smat *lq1;
+	struct blasfeo_svec *tmp_m;
+	struct blasfeo_smat *A_LQ;
+	struct blasfeo_smat *A_Q;
+	struct blasfeo_smat *Zt;
+	struct blasfeo_smat *ZtH;
+	struct blasfeo_smat *ZtHZ;
+	struct blasfeo_svec *xy;
+	struct blasfeo_svec *Yxy;
+	struct blasfeo_svec *xz;
+	struct blasfeo_svec *tmp_nv;
+	struct blasfeo_svec *tmp_2ns;
+	struct blasfeo_svec *tmp_nv2ns;
+	struct blasfeo_smat *A_li; // A of linearly independent equality constraints
+	struct blasfeo_svec *b_li; // b of linearly independent equality constraints
+	struct blasfeo_smat *A_bkp; // pointer to backup A
+	struct blasfeo_svec *b_bkp; // pointer to backup b
+	struct blasfeo_smat *Ab_LU;
+	float *stat; // convergence statistics
+	int *ipiv_v;
+	int *ipiv_e;
+	int *ipiv_e1;
+	void *lq_work0;
+	void *lq_work1;
+	void *lq_work_null;
+	void *orglq_work_null;
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // numer of recorded stat per ipm iter
+	int scale;
+	int use_hess_fact;
+	int use_A_fact;
+	int status;
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	int ne_li; // number of linearly independent equality constraints
+	int ne_bkp; // ne backup
+	hpipm_size_t memsize; // memory size (in bytes) of workspace
+	};
+
+
+
+//
+hpipm_size_t s_dense_qp_ipm_arg_memsize(struct s_dense_qp_dim *qp_dim);
+//
+void s_dense_qp_ipm_arg_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, void *mem);
+//
+void s_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set(char *field, void *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_mu0(float *mu0, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_lam_min(float *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_t_min(float *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_tau_min(float *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_compute_obj(int *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_split_step(int *value, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qp_ipm_arg *arg);
+
+//
+hpipm_size_t s_dense_qp_ipm_ws_memsize(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_ipm_ws_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws, void *mem);
+//
+void s_dense_qp_ipm_get(char *field, struct s_dense_qp_ipm_ws *ws, void *value);
+//
+void s_dense_qp_ipm_get_status(struct s_dense_qp_ipm_ws *ws, int *status);
+//
+void s_dense_qp_ipm_get_iter(struct s_dense_qp_ipm_ws *ws, int *iter);
+//
+void s_dense_qp_ipm_get_max_res_stat(struct s_dense_qp_ipm_ws *ws, float *res_stat);
+//
+void s_dense_qp_ipm_get_max_res_eq(struct s_dense_qp_ipm_ws *ws, float *res_eq);
+//
+void s_dense_qp_ipm_get_max_res_ineq(struct s_dense_qp_ipm_ws *ws, float *res_ineq);
+//
+void s_dense_qp_ipm_get_max_res_comp(struct s_dense_qp_ipm_ws *ws, float *res_comp);
+//
+void s_dense_qp_ipm_get_stat(struct s_dense_qp_ipm_ws *ws, float **stat);
+//
+void s_dense_qp_ipm_get_stat_m(struct s_dense_qp_ipm_ws *ws, int *stat_m);
+//
+void s_dense_qp_init_var(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_ipm_abs_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_ipm_delta_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_ipm_solve(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_ipm_predict(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_ipm_sens(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_compute_step_length(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h
new file mode 100644
index 000000000..260dc0ab2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h
@@ -0,0 +1,72 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QP_KKT_H_
+#define HPIPM_S_DENSE_QP_KKT_H_
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_fact_solve_kkt_unconstr_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_fact_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_fact_lq_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_remove_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_restore_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+//
+void s_dense_qp_compute_obj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_KKT_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h
new file mode 100644
index 000000000..06b609c53
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h
@@ -0,0 +1,106 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QP_RES_H_
+#define HPIPM_S_DENSE_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_s_dense_qp_dim.h>
+#include <hpipm_s_dense_qp.h>
+#include <hpipm_s_dense_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qp_res
+	{
+	struct s_dense_qp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // max of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_dense_qp_res_ws
+	{
+	struct blasfeo_svec *tmp_nbg; // work space of size nbM+ngM
+	struct blasfeo_svec *tmp_ns; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qp_res_memsize(struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_res_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res *res, void *mem);
+//
+hpipm_size_t s_dense_qp_res_ws_memsize(struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_res_ws_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res_ws *workspace, void *mem);
+//
+void s_dense_qp_res_compute(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws);
+//
+void s_dense_qp_res_compute_lin(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_sol *qp_step, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws);
+//
+void s_dense_qp_res_compute_inf_norm(struct s_dense_qp_res *res);
+//
+void s_dense_qp_res_get_all(struct s_dense_qp_res *res, float *res_g, float *res_ls, float *res_us, float *res_b, float *res_d_lb, float *res_d_ub, float *res_d_lg, float *res_d_ug, float *res_d_ls, float *res_d_us, float *res_m_lb, float *res_m_ub, float *res_m_lg, float *res_m_ug, float *res_m_ls, float *res_m_us);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_D_DENSE_QP_RES_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h
new file mode 100644
index 000000000..1f4007637
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h
@@ -0,0 +1,94 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_DENSE_QP_SOL_H_
+#define HPIPM_S_DENSE_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_dense_qp_sol
+	{
+	struct s_dense_qp_dim *dim;
+	struct blasfeo_svec *v;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	void *misc;
+	float obj;
+	int valid_obj;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_dense_qp_sol_memsize(struct s_dense_qp_dim *dim);
+//
+void s_dense_qp_sol_create(struct s_dense_qp_dim *dim, struct s_dense_qp_sol *qp_sol, void *memory);
+//
+void s_dense_qp_sol_get_all(struct s_dense_qp_sol *qp_sol, float *v, float *ls, float *us, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug, float *lam_ls, float *lam_us);
+//
+void s_dense_qp_sol_get(char *field, struct s_dense_qp_sol *sol, void *value);
+//
+void s_dense_qp_sol_get_v(struct s_dense_qp_sol *sol, float *v);
+//
+void s_dense_qp_sol_get_valid_obj(struct s_dense_qp_sol *sol, int *valid_obj);
+//
+void s_dense_qp_sol_get_obj(struct s_dense_qp_sol *sol, float *obj);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h
new file mode 100644
index 000000000..3dd93259a
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h
@@ -0,0 +1,84 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_DENSE_QP_UTILS_H_
+#define HPIPM_S_DENSE_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_dense_qp_dim.h"
+#include "hpipm_s_dense_qp.h"
+#include "hpipm_s_dense_qp_sol.h"
+#include "hpipm_s_dense_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_dense_qp_dim_print(struct s_dense_qp_dim *qp_dim);
+//
+//void s_dense_qp_dim_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim);
+//
+void s_dense_qp_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp);
+//
+//void s_dense_qp_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp);
+//
+void s_dense_qp_sol_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_sol *dense_qp_sol);
+//
+//void s_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg);
+//
+void s_dense_qp_res_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_res *dense_qp_res);
+//
+void s_dense_qp_arg_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *qp_ipm_arg);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_DENSE_QP_UTILS_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h
new file mode 100644
index 000000000..b90b2ac63
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h
@@ -0,0 +1,303 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_H_
+#define HPIPM_S_OCP_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qcqp
+	{
+	struct s_ocp_qcqp_dim *dim;
+	struct blasfeo_smat *BAbt; // dynamics matrix & vector work space
+	struct blasfeo_smat *RSQrq; // hessian of cost & vector work space
+	struct blasfeo_smat *DCt; // inequality constraints matrix
+	struct blasfeo_smat **Hq; // hessians of quadratic constraints
+	struct blasfeo_svec *b; // dynamics vector
+	struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks
+	struct blasfeo_svec *d; // inequality constraints vector
+	struct blasfeo_svec *d_mask; // inequality constraints mask vector
+	struct blasfeo_svec *m; // rhs of complementarity condition
+	struct blasfeo_svec *Z; // (diagonal) hessian of slacks
+	int **idxb; // indices of box constrained variables within [u; x]
+	int **idxs_rev; // index of soft constraints (reverse storage)
+	int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qcqp_strsize();
+//
+hpipm_size_t s_ocp_qcqp_memsize(struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp *qp, void *memory);
+//
+void s_ocp_qcqp_copy_all(struct s_ocp_qcqp *qp_orig, struct s_ocp_qcqp *qp_dest);
+
+// setters
+//
+void s_ocp_qcqp_set_all_zero(struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_rhs_zero(struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set(char *fiels_name, int stage, void *value, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_A(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_B(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_b(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Q(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_S(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_R(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_q(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_r(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lb(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lb_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ub(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ub_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lbx(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lbx_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ubx(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ubx_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lbu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lbu_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ubu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ubu_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_idxb(int stage, int *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_idxbx(int stage, int *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jbx(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_idxbu(int stage, int *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jbu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_C(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_D(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lg(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lg_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ug(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_ug_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Qq(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Sq(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Rq(int stage, float *mat, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_qq(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_rq(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_uq(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_uq_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Zl(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Zu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_zl(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_zu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lls(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lls_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lus(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_lus_mask(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_idxs(int stage, int *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jsbu(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jsbx(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jsg(int stage, float *vec, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_set_Jsq(int stage, float *vec, struct s_ocp_qcqp *qp);
+
+// getters
+//
+void s_ocp_qcqp_get(char *field, int stage, struct s_ocp_qcqp *qp, void *value);
+//
+void s_ocp_qcqp_get_A(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_B(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_b(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_Q(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_S(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_R(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_q(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_r(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ub(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ub_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lb(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lb_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lbx(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lbx_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ubx(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ubx_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lbu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lbu_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ubu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ubu_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_idxb(int stage, struct s_ocp_qcqp *qp, int *vec);
+//
+//void s_ocp_qcqp_get_idxbx(int stage, struct s_ocp_qcqp *qp, int *vec);
+//
+//void s_ocp_qcqp_get_Jbx(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+//void s_ocp_qcqp_get_idxbu(int stage, struct s_ocp_qcqp *qp, int *vec);
+//
+//void s_ocp_qcqp_get_Jbu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_C(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_D(int stage, struct s_ocp_qcqp *qp, float *mat);
+//
+void s_ocp_qcqp_get_lg(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lg_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ug(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_ug_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_Zl(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_Zu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_zl(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_zu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lls(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lls_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lus(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+void s_ocp_qcqp_get_lus_mask(int stage, struct s_ocp_qcqp *qp, float *vec);
+// XXX only valid if there is one slack per softed constraint !!!
+void s_ocp_qcqp_get_idxs(int stage, struct s_ocp_qcqp *qp, int *vec);
+//
+void s_ocp_qcqp_get_idxs_rev(int stage, struct s_ocp_qcqp *qp, int *vec);
+//
+//void s_ocp_qcqp_get_Jsbu(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+//void s_ocp_qcqp_get_Jsbx(int stage, struct s_ocp_qcqp *qp, float *vec);
+//
+//void s_ocp_qcqp_get_Jsg(int stage, struct s_ocp_qcqp *qp, float *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h
new file mode 100644
index 000000000..c09903f07
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h
@@ -0,0 +1,119 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_DIM_H_
+#define HPIPM_S_OCP_QCQP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qcqp_dim
+	{
+	struct s_ocp_qp_dim *qp_dim; // dim of qp approximation
+	int *nx; // number of states
+	int *nu; // number of inputs
+	int *nb; // number of box constraints
+	int *nbx; // number of (two-sided) state box constraints
+	int *nbu; // number of (two-sided) input box constraints
+	int *ng; // number of (two-sided) general constraints
+	int *nq; // number of (upper) quadratic constraints
+	int *ns; // number of soft constraints
+	int *nsbx; // number of (two-sided) soft state box constraints
+	int *nsbu; // number of (two-sided) soft input box constraints
+	int *nsg; // number of (two-sided) soft general constraints
+	int *nsq; // number of (upper) soft quadratic constraints
+	int N; // horizon length
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qcqp_dim_strsize();
+//
+hpipm_size_t s_ocp_qcqp_dim_memsize(int N);
+//
+void s_ocp_qcqp_dim_create(int N, struct s_ocp_qcqp_dim *qp_dim, void *memory);
+//
+void s_ocp_qcqp_dim_copy_all(struct s_ocp_qcqp_dim *dim_orig, struct s_ocp_qcqp_dim *dim_dest);
+//
+void s_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nx(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nu(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_ng(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nq(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_ns(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_dim_get(struct s_ocp_qcqp_dim *dim, char *field, int stage, int *value);
+//
+void s_ocp_qcqp_dim_get_N(struct s_ocp_qcqp_dim *dim, int *value);
+//
+void s_ocp_qcqp_dim_get_nx(struct s_ocp_qcqp_dim *dim, int stage, int *value);
+//
+void s_ocp_qcqp_dim_get_nu(struct s_ocp_qcqp_dim *dim, int stage, int *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QCQP_DIM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h
new file mode 100644
index 000000000..c14fc1c9f
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h
@@ -0,0 +1,191 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_IPM_H_
+#define HPIPM_S_OCP_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_ocp_qcqp_dim.h>
+#include <hpipm_s_ocp_qp.h>
+#include <hpipm_s_ocp_qcqp_res.h>
+#include <hpipm_s_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qcqp_ipm_arg
+	{
+	struct s_ocp_qp_ipm_arg *qp_arg;
+	float mu0; // initial value for complementarity slackness
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_ocp_qcqp_ipm_ws
+	{
+	struct s_ocp_qp_ipm_ws *qp_ws;
+	struct s_ocp_qp *qp;
+	struct s_ocp_qp_sol *qp_sol;
+	struct s_ocp_qcqp_res_ws *qcqp_res_ws;
+	struct s_ocp_qcqp_res *qcqp_res;
+	struct blasfeo_svec *tmp_nuxM;
+	int iter; // iteration number
+	int status;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qcqp_ipm_arg_strsize();
+//
+hpipm_size_t s_ocp_qcqp_ipm_arg_memsize(struct s_ocp_qcqp_dim *ocp_dim);
+//
+void s_ocp_qcqp_ipm_arg_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, void *mem);
+//
+void s_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qcqp_ipm_arg *arg);
+//
+void s_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set maximum number of iterations
+void s_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set minimum step lenght
+void s_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set initial value of barrier parameter
+void s_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void s_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void s_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void s_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void s_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set regularization of primal variables
+void s_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void s_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void s_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void s_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+void s_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// compute residuals after solution
+void s_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// compute residuals of prediction
+void s_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// min value of lam in the solution
+void s_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// min value of t in the solution
+void s_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_ocp_qcqp_ipm_arg *arg);
+// use different step for primal and dual variables
+void s_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void s_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t s_ocp_qcqp_ipm_ws_strsize();
+//
+hpipm_size_t s_ocp_qcqp_ipm_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg);
+//
+void s_ocp_qcqp_ipm_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws, void *mem);
+//
+void s_ocp_qcqp_ipm_get(char *field, struct s_ocp_qcqp_ipm_ws *ws, void *value);
+//
+void s_ocp_qcqp_ipm_get_status(struct s_ocp_qcqp_ipm_ws *ws, int *status);
+//
+void s_ocp_qcqp_ipm_get_iter(struct s_ocp_qcqp_ipm_ws *ws, int *iter);
+//
+void s_ocp_qcqp_ipm_get_max_res_stat(struct s_ocp_qcqp_ipm_ws *ws, float *res_stat);
+//
+void s_ocp_qcqp_ipm_get_max_res_eq(struct s_ocp_qcqp_ipm_ws *ws, float *res_eq);
+//
+void s_ocp_qcqp_ipm_get_max_res_ineq(struct s_ocp_qcqp_ipm_ws *ws, float *res_ineq);
+//
+void s_ocp_qcqp_ipm_get_max_res_comp(struct s_ocp_qcqp_ipm_ws *ws, float *res_comp);
+//
+void s_ocp_qcqp_ipm_get_stat(struct s_ocp_qcqp_ipm_ws *ws, float **stat);
+//
+void s_ocp_qcqp_ipm_get_stat_m(struct s_ocp_qcqp_ipm_ws *ws, int *stat_m);
+//
+void s_ocp_qcqp_init_var(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws);
+//
+void s_ocp_qcqp_ipm_solve(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_OCP_QCQP_IPM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h
new file mode 100644
index 000000000..1ceeec93b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h
@@ -0,0 +1,115 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_RES_H_
+#define HPIPM_S_OCP_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_ocp_qcqp_dim.h>
+#include <hpipm_s_ocp_qcqp.h>
+#include <hpipm_s_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qcqp_res
+	{
+	struct s_ocp_qcqp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // max of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_ocp_qcqp_res_ws
+	{
+	struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM
+	struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	struct blasfeo_svec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_svec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qcqp_res_memsize(struct s_ocp_qcqp_dim *ocp_dim);
+//
+void s_ocp_qcqp_res_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res *res, void *mem);
+//
+hpipm_size_t s_ocp_qcqp_res_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim);
+//
+void s_ocp_qcqp_res_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res_ws *workspace, void *mem);
+//
+void s_ocp_qcqp_res_compute(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_res *res, struct s_ocp_qcqp_res_ws *ws);
+//
+void s_ocp_qcqp_res_compute_inf_norm(struct s_ocp_qcqp_res *res);
+//
+void s_ocp_qcqp_res_get_max_res_stat(struct s_ocp_qcqp_res *res, float *value);
+//
+void s_ocp_qcqp_res_get_max_res_eq(struct s_ocp_qcqp_res *res, float *value);
+//
+void s_ocp_qcqp_res_get_max_res_ineq(struct s_ocp_qcqp_res *res, float *value);
+//
+void s_ocp_qcqp_res_get_max_res_comp(struct s_ocp_qcqp_res *res, float *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_OCP_QCQP_RES_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h
new file mode 100644
index 000000000..3d58022cc
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h
@@ -0,0 +1,115 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_SOL_H_
+#define HPIPM_S_OCP_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qcqp_sol
+	{
+	struct s_ocp_qcqp_dim *dim;
+	struct blasfeo_svec *ux;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qcqp_sol_strsize();
+//
+hpipm_size_t s_ocp_qcqp_sol_memsize(struct s_ocp_qcqp_dim *dim);
+//
+void s_ocp_qcqp_sol_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp_sol *qp_sol, void *memory);
+//
+void s_ocp_qcqp_sol_copy_all(struct s_ocp_qcqp_sol *qp_sol_orig, struct s_ocp_qcqp_sol *qp_sol_dest);
+//
+void s_ocp_qcqp_sol_get(char *field, int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_u(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_x(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_sl(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_su(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_pi(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_lam_lb(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_lam_ub(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_lam_lg(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_get_lam_ug(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_ocp_qcqp_sol_set(char *field, int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol);
+//
+void s_ocp_qcqp_sol_set_u(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol);
+//
+void s_ocp_qcqp_sol_set_x(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol);
+//
+void s_ocp_qcqp_sol_set_sl(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol);
+//
+void s_ocp_qcqp_sol_set_su(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QCQP_SOL_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h
new file mode 100644
index 000000000..d64e3aabe
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h
@@ -0,0 +1,82 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QCQP_UTILS_H_
+#define HPIPM_S_OCP_QCQP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qcqp_dim.h"
+#include "hpipm_s_ocp_qp.h"
+#include "hpipm_s_ocp_qcqp_sol.h"
+#include "hpipm_s_ocp_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_ocp_qcqp_dim_print(struct s_ocp_qcqp_dim *qcqp_dim);
+//
+void s_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim);
+//
+void s_ocp_qcqp_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp);
+//
+void s_ocp_qcqp_sol_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_sol *ocp_qcqp_sol);
+//
+void s_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_ipm_arg *arg);
+//
+void s_ocp_qcqp_res_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_res *ocp_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QCQP_UTILS_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h
new file mode 100644
index 000000000..b49191f19
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h
@@ -0,0 +1,306 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_H_
+#define HPIPM_S_OCP_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp
+	{
+	struct s_ocp_qp_dim *dim;
+	struct blasfeo_smat *BAbt; // dynamics matrix & vector work space
+	struct blasfeo_smat *RSQrq; // hessian of cost & vector work space
+	struct blasfeo_smat *DCt; // inequality constraints matrix
+	struct blasfeo_svec *b; // dynamics vector
+	struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks
+	struct blasfeo_svec *d; // inequality constraints vector
+	struct blasfeo_svec *d_mask; // inequality constraints mask vector
+	struct blasfeo_svec *m; // rhs of complementarity condition
+	struct blasfeo_svec *Z; // (diagonal) hessian of slacks
+	int **idxb; // indices of box constrained variables within [u; x]
+	int **idxs_rev; // index of soft constraints (reverse storage)
+	int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1]
+	int *diag_H_flag; // flag the fact that Hessian is diagonal
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qp_strsize();
+//
+hpipm_size_t s_ocp_qp_memsize(struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp *qp, void *memory);
+//
+void s_ocp_qp_copy_all(struct s_ocp_qp *qp_orig, struct s_ocp_qp *qp_dest);
+
+// setters
+//
+void s_ocp_qp_set_all_zero(struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_rhs_zero(struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_all_rowmaj(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set(char *fiels_name, int stage, void *value, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_A(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_B(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_b(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Q(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_S(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_R(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_q(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_r(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lb(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lb_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ub(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ub_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lbx(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lbx_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ubx(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ubx_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lbu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lbu_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ubu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ubu_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxb(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxbx(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jbx(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxbu(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jbu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_C(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_D(int stage, float *mat, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lg(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lg_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ug(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_ug_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Zl(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Zu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_zl(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_zu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lls(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lls_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lus(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_lus_mask(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxs(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxs_rev(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jsbu(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jsbx(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jsg(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxe(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxbxe(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxbue(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_idxge(int stage, int *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jbxe(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jbue(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_Jge(int stage, float *vec, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_set_diag_H_flag(int stage, int *value, struct s_ocp_qp *qp);
+
+// getters
+//
+void s_ocp_qp_get(char *field, int stage, struct s_ocp_qp *qp, void *value);
+//
+void s_ocp_qp_get_A(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_B(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_b(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_Q(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_S(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_R(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_q(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_r(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ub(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ub_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lb(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lb_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lbx(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lbx_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ubx(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ubx_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lbu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lbu_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ubu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ubu_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_idxb(int stage, struct s_ocp_qp *qp, int *vec);
+//
+//void s_ocp_qp_get_idxbx(int stage, struct s_ocp_qp *qp, int *vec);
+//
+//void s_ocp_qp_get_Jbx(int stage, struct s_ocp_qp *qp, float *vec);
+//
+//void s_ocp_qp_get_idxbu(int stage, struct s_ocp_qp *qp, int *vec);
+//
+//void s_ocp_qp_get_Jbu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_C(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_D(int stage, struct s_ocp_qp *qp, float *mat);
+//
+void s_ocp_qp_get_lg(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lg_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ug(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_ug_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_Zl(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_Zu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_zl(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_zu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lls(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lls_mask(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lus(int stage, struct s_ocp_qp *qp, float *vec);
+//
+void s_ocp_qp_get_lus_mask(int stage, struct s_ocp_qp *qp, float *vec);
+// XXX only valid if there is one slack per softed constraint !!!
+void s_ocp_qp_get_idxs(int stage, struct s_ocp_qp *qp, int *vec);
+//
+void s_ocp_qp_get_idxs_rev(int stage, struct s_ocp_qp *qp, int *vec);
+//
+//void s_ocp_qp_get_Jsbu(int stage, struct s_ocp_qp *qp, float *vec);
+//
+//void s_ocp_qp_get_Jsbx(int stage, struct s_ocp_qp *qp, float *vec);
+//
+//void s_ocp_qp_get_Jsg(int stage, struct s_ocp_qp *qp, float *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h
new file mode 100644
index 000000000..bce80243b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h
@@ -0,0 +1,141 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_DIM_H_
+#define HPIPM_S_OCP_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp_dim
+	{
+	int *nx; // number of states
+	int *nu; // number of inputs
+	int *nb; // number of box constraints
+	int *nbx; // number of state box constraints
+	int *nbu; // number of input box constraints
+	int *ng; // number of general constraints
+	int *ns; // number of soft constraints
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int *nbxe; // number of state box constraints which are equality
+	int *nbue; // number of input box constraints which are equality
+	int *nge; // number of general constraints which are equality
+	int N; // horizon length
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qp_dim_strsize();
+//
+hpipm_size_t s_ocp_qp_dim_memsize(int N);
+//
+void s_ocp_qp_dim_create(int N, struct s_ocp_qp_dim *qp_dim, void *memory);
+//
+void s_ocp_qp_dim_copy_all(struct s_ocp_qp_dim *dim_orig, struct s_ocp_qp_dim *dim_dest);
+//
+void s_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set(char *field, int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nx(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nu(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nbx(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nbu(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_ng(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_ns(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nsbx(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nsbu(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nsg(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nbxe(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nbue(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_set_nge(int stage, int value, struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_dim_get(struct s_ocp_qp_dim *dim, char *field, int stage, int *value);
+//
+void s_ocp_qp_dim_get_N(struct s_ocp_qp_dim *dim, int *value);
+//
+void s_ocp_qp_dim_get_nx(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nu(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nbx(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nbu(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_ng(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_ns(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nsbx(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nsbu(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nsg(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nbxe(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nbue(struct s_ocp_qp_dim *dim, int stage, int *value);
+//
+void s_ocp_qp_dim_get_nge(struct s_ocp_qp_dim *dim, int stage, int *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h
new file mode 100644
index 000000000..11f3c47be
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h
@@ -0,0 +1,250 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_IPM_H_
+#define HPIPM_S_OCP_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_ocp_qp_dim.h>
+#include <hpipm_s_ocp_qp.h>
+#include <hpipm_s_ocp_qp_res.h>
+#include <hpipm_s_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp_ipm_arg
+	{
+	float mu0; // initial value for complementarity slackness
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	float tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different steps for primal and dual variables
+	int var_init_scheme; // variables initialization scheme
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_ocp_qp_ipm_ws
+	{
+	struct s_core_qp_ipm_workspace *core_workspace;
+	struct s_ocp_qp_res_ws *res_workspace;
+	struct s_ocp_qp_dim *dim; // cache dim
+	struct s_ocp_qp_sol *sol_step;
+	struct s_ocp_qp_sol *sol_itref;
+	struct s_ocp_qp *qp_step;
+	struct s_ocp_qp *qp_itref;
+	struct s_ocp_qp_res *res;
+	struct s_ocp_qp_res *res_itref;
+	struct blasfeo_svec *Gamma; // hessian update
+	struct blasfeo_svec *gamma; // hessian update
+	struct blasfeo_svec *tmp_nuxM; // work space of size nxM
+	struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	struct blasfeo_svec *Pb; // Pb
+	struct blasfeo_svec *Zs_inv;
+	struct blasfeo_svec *tmp_m;
+	struct blasfeo_svec *l; // cache linear part for _get_ric_xxx
+	struct blasfeo_smat *L;
+	struct blasfeo_smat *Ls;
+	struct blasfeo_smat *P;
+	struct blasfeo_smat *Lh;
+	struct blasfeo_smat *AL;
+	struct blasfeo_smat *lq0;
+	struct blasfeo_smat *tmp_nxM_nxM;
+	float *stat; // convergence statistics
+	int *use_hess_fact;
+	void *lq_work0;
+	float qp_res[4]; // infinity norm of residuals
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // number of recorded stat per IPM iter
+	int use_Pb;
+	int status; // solver status
+	int square_root_alg; // cache from arg
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	int valid_ric_vec; // meaningful riccati vectors
+	int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qp_ipm_arg_strsize();
+//
+hpipm_size_t s_ocp_qp_ipm_arg_memsize(struct s_ocp_qp_dim *ocp_dim);
+//
+void s_ocp_qp_ipm_arg_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, void *mem);
+//
+void s_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qp_ipm_arg *arg);
+//
+void s_ocp_qp_ipm_arg_set(char *field, void *value, struct s_ocp_qp_ipm_arg *arg);
+// set maximum number of iterations
+void s_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_ocp_qp_ipm_arg *arg);
+// set minimum step lenght
+void s_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_ocp_qp_ipm_arg *arg);
+// set initial value of barrier parameter
+void s_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_ocp_qp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void s_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_ocp_qp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void s_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_ocp_qp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void s_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_ocp_qp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void s_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_ocp_qp_ipm_arg *arg);
+// set regularization of primal variables
+void s_ocp_qp_ipm_arg_set_reg_prim(float *tol_comp, struct s_ocp_qp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void s_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_ocp_qp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void s_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_ocp_qp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void s_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+void s_ocp_qp_ipm_arg_set_ric_alg(int *alg, struct s_ocp_qp_ipm_arg *arg);
+// dual solution of equality constraints (only for abs_form==1)
+void s_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_ocp_qp_ipm_arg *arg);
+// compute residuals after solution
+void s_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qp_ipm_arg *arg);
+// compute residuals of prediction
+void s_ocp_qp_ipm_arg_set_comp_res_pred(int *alg, struct s_ocp_qp_ipm_arg *arg);
+// min value of lam in the solution
+void s_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_ocp_qp_ipm_arg *arg);
+// min value of t in the solution
+void s_ocp_qp_ipm_arg_set_t_min(float *value, struct s_ocp_qp_ipm_arg *arg);
+// min value of tau in the solution
+void s_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_ocp_qp_ipm_arg *arg);
+// set split step: 0 same step, 1 different step for primal and dual variables
+void s_ocp_qp_ipm_arg_set_split_step(int *value, struct s_ocp_qp_ipm_arg *arg);
+// variables initialization scheme
+void s_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct s_ocp_qp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void s_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qp_ipm_arg *arg);
+
+//
+hpipm_size_t s_ocp_qp_ipm_ws_strsize();
+//
+hpipm_size_t s_ocp_qp_ipm_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg);
+//
+void s_ocp_qp_ipm_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, void *mem);
+//
+void s_ocp_qp_ipm_get(char *field, struct s_ocp_qp_ipm_ws *ws, void *value);
+//
+void s_ocp_qp_ipm_get_status(struct s_ocp_qp_ipm_ws *ws, int *status);
+//
+void s_ocp_qp_ipm_get_iter(struct s_ocp_qp_ipm_ws *ws, int *iter);
+//
+void s_ocp_qp_ipm_get_max_res_stat(struct s_ocp_qp_ipm_ws *ws, float *res_stat);
+//
+void s_ocp_qp_ipm_get_max_res_eq(struct s_ocp_qp_ipm_ws *ws, float *res_eq);
+//
+void s_ocp_qp_ipm_get_max_res_ineq(struct s_ocp_qp_ipm_ws *ws, float *res_ineq);
+//
+void s_ocp_qp_ipm_get_max_res_comp(struct s_ocp_qp_ipm_ws *ws, float *res_comp);
+//
+void s_ocp_qp_ipm_get_stat(struct s_ocp_qp_ipm_ws *ws, float **stat);
+//
+void s_ocp_qp_ipm_get_stat_m(struct s_ocp_qp_ipm_ws *ws, int *stat_m);
+//
+void s_ocp_qp_ipm_get_ric_Lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Lr);
+//
+void s_ocp_qp_ipm_get_ric_Ls(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Ls);
+//
+void s_ocp_qp_ipm_get_ric_P(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *P);
+//
+void s_ocp_qp_ipm_get_ric_lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *lr);
+//
+void s_ocp_qp_ipm_get_ric_p(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *p);
+// feedback control gain in the form u = K x + k
+void s_ocp_qp_ipm_get_ric_K(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *K);
+// feedback control gain in the form u = K x + k
+void s_ocp_qp_ipm_get_ric_k(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *k);
+//
+void s_ocp_qp_init_var(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_ipm_abs_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_ipm_delta_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_ipm_solve(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_ipm_predict(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_ipm_sens(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h
new file mode 100644
index 000000000..3eb1f4aae
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h
@@ -0,0 +1,66 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+#ifndef HPIPM_S_OCP_QP_KKT_H_
+#define HPIPM_S_OCP_QP_KKT_H_
+
+#include <hpipm_common.h>
+#include <hpipm_s_ocp_qp.h>
+#include <hpipm_s_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+void s_ocp_qp_fact_solve_kkt_unconstr(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_fact_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_fact_lq_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+//
+void s_ocp_qp_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+#endif // HPIPM_S_OCP_QP_KKT_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h
new file mode 100644
index 000000000..5a7b3b070
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h
@@ -0,0 +1,118 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_RED_H_
+#define HPIPM_S_OCP_QP_RED_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_s_ocp_qp_dim.h>
+#include <hpipm_s_ocp_qp.h>
+#include <hpipm_s_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp_reduce_eq_dof_arg
+	{
+	float lam_min;
+	float t_min;
+	int alias_unchanged; // do not keep copy unchanged stage
+	int comp_prim_sol; // primal solution (v)
+	int comp_dual_sol_eq; // dual solution equality constr (pi)
+	int comp_dual_sol_ineq; // dual solution inequality constr (lam t)
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+struct s_ocp_qp_reduce_eq_dof_ws
+	{
+	struct blasfeo_svec *tmp_nuxM;
+	struct blasfeo_svec *tmp_nbgM;
+	int *e_imask_ux;
+	int *e_imask_d;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+void s_ocp_qp_dim_reduce_eq_dof(struct s_ocp_qp_dim *dim, struct s_ocp_qp_dim *dim_red);
+//
+hpipm_size_t s_ocp_qp_reduce_eq_dof_arg_memsize();
+//
+void s_ocp_qp_reduce_eq_dof_arg_create(struct s_ocp_qp_reduce_eq_dof_arg *arg, void *mem);
+//
+void s_ocp_qp_reduce_eq_dof_arg_set_default(struct s_ocp_qp_reduce_eq_dof_arg *arg);
+//
+void s_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void s_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value);
+//
+hpipm_size_t s_ocp_qp_reduce_eq_dof_ws_memsize(struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_reduce_eq_dof_ws_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_reduce_eq_dof_ws *work, void *mem);
+//
+void s_ocp_qp_reduce_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work);
+//
+void s_ocp_qp_reduce_eq_dof_lhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work);
+//
+void s_ocp_qp_reduce_eq_dof_rhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work);
+//
+void s_ocp_qp_restore_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol_red, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QP_RED_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h
new file mode 100644
index 000000000..821585da6
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h
@@ -0,0 +1,114 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_RES_H_
+#define HPIPM_S_OCP_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_ocp_qp_dim.h>
+#include <hpipm_s_ocp_qp.h>
+#include <hpipm_s_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp_res
+	{
+	struct s_ocp_qp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // max of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_ocp_qp_res_ws
+	{
+	struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qp_res_memsize(struct s_ocp_qp_dim *ocp_dim);
+//
+void s_ocp_qp_res_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res *res, void *mem);
+//
+hpipm_size_t s_ocp_qp_res_ws_memsize(struct s_ocp_qp_dim *ocp_dim);
+//
+void s_ocp_qp_res_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res_ws *workspace, void *mem);
+//
+void s_ocp_qp_res_compute(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws);
+//
+void s_ocp_qp_res_compute_lin(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_sol *qp_step, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws);
+//
+void s_ocp_qp_res_compute_inf_norm(struct s_ocp_qp_res *res);
+//
+void s_ocp_qp_res_get_all(struct s_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us);
+//
+void s_ocp_qp_res_get_max_res_stat(struct s_ocp_qp_res *res, float *value);
+//
+void s_ocp_qp_res_get_max_res_eq(struct s_ocp_qp_res *res, float *value);
+//
+void s_ocp_qp_res_get_max_res_ineq(struct s_ocp_qp_res *res, float *value);
+//
+void s_ocp_qp_res_get_max_res_comp(struct s_ocp_qp_res *res, float *value);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_OCP_QP_RES_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h
new file mode 100644
index 000000000..94dfa0d00
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h
@@ -0,0 +1,128 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_SOL_H_
+#define HPIPM_S_OCP_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_ocp_qp_sol
+	{
+	struct s_ocp_qp_dim *dim;
+	struct blasfeo_svec *ux;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_ocp_qp_sol_strsize();
+//
+hpipm_size_t s_ocp_qp_sol_memsize(struct s_ocp_qp_dim *dim);
+//
+void s_ocp_qp_sol_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_sol *qp_sol, void *memory);
+//
+void s_ocp_qp_sol_copy_all(struct s_ocp_qp_sol *qp_sol_orig, struct s_ocp_qp_sol *qp_sol_dest);
+//
+void s_qp_sol_get_all(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us);
+//
+void s_qp_sol_get_all_rowmaj(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us);
+//
+void s_ocp_qp_sol_set_all(float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us, struct s_ocp_qp_sol *qp_sol);
+//
+void s_ocp_qp_sol_get(char *field, int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_u(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_x(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_sl(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_su(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_pi(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_lb(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_lbu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_lbx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_ub(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_ubu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_ubx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_lg(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_get_lam_ug(int stage, struct s_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_ocp_qp_sol_set(char *field, int stage, float *vec, struct s_ocp_qp_sol *qp_sol);
+//
+void s_ocp_qp_sol_set_u(int stage, float *vec, struct s_ocp_qp_sol *qp_sol);
+//
+void s_ocp_qp_sol_set_x(int stage, float *vec, struct s_ocp_qp_sol *qp_sol);
+//
+void s_ocp_qp_sol_set_sl(int stage, float *vec, struct s_ocp_qp_sol *qp_sol);
+//
+void s_ocp_qp_sol_set_su(int stage, float *vec, struct s_ocp_qp_sol *qp_sol);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_OCP_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h
new file mode 100644
index 000000000..a4f832a5e
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_OCP_QP_UTILS_H_
+#define HPIPM_S_OCP_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_ocp_qp_dim.h"
+#include "hpipm_s_ocp_qp.h"
+#include "hpipm_s_ocp_qp_sol.h"
+#include "hpipm_s_ocp_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_ocp_qp_dim_print(struct s_ocp_qp_dim *qp_dim);
+//
+void s_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim);
+//
+void s_ocp_qp_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp);
+//
+void s_ocp_qp_sol_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_sol *ocp_qp_sol);
+//
+void s_ocp_qp_ipm_arg_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg);
+//
+void s_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg);
+//
+void s_ocp_qp_res_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_res *ocp_qp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_D_OCP_QP_UTILS_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h b/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h
new file mode 100644
index 000000000..e40511e69
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h
@@ -0,0 +1,115 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_PART_COND_H_
+#define HPIPM_S_PART_COND_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_cond.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_part_cond_qp_arg
+	{
+	struct s_cond_qp_arg *cond_arg;
+	int N2;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_part_cond_qp_ws
+	{
+	struct s_cond_qp_ws *cond_workspace;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_part_cond_qp_arg_memsize(int N2);
+//
+void s_part_cond_qp_arg_create(int N2, struct s_part_cond_qp_arg *cond_arg, void *mem);
+//
+void s_part_cond_qp_arg_set_default(struct s_part_cond_qp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 squre-root
+void s_part_cond_qp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qp_arg *cond_arg);
+//
+void s_part_cond_qp_arg_set_comp_prim_sol(int value, struct s_part_cond_qp_arg *cond_arg);
+//
+void s_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_part_cond_qp_arg *cond_arg);
+//
+void s_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_part_cond_qp_arg *cond_arg);
+
+//
+void s_part_cond_qp_compute_block_size(int N, int N2, int *block_size);
+//
+void s_part_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim);
+//
+hpipm_size_t s_part_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg);
+//
+void s_part_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws, void *mem);
+//
+void s_part_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws);
+//
+void s_part_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws);
+//
+void s_part_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws);
+//
+void s_part_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_ocp_qp_sol *part_dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws);
+
+//
+void s_part_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_PART_COND_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h
new file mode 100644
index 000000000..311f7000b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h
@@ -0,0 +1,107 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_PART_COND_QCQP_H_
+#define HPIPM_S_PART_COND_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_cond_qcqp.h"
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_part_cond_qcqp_arg
+	{
+	struct s_cond_qcqp_arg *cond_arg;
+	int N2;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_part_cond_qcqp_ws
+	{
+	struct s_cond_qcqp_ws *cond_ws;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_part_cond_qcqp_arg_memsize(int N2);
+//
+void s_part_cond_qcqp_arg_create(int N2, struct s_part_cond_qcqp_arg *cond_arg, void *mem);
+//
+void s_part_cond_qcqp_arg_set_default(struct s_part_cond_qcqp_arg *cond_arg);
+// set riccati-like algorithm: 0 classical, 1 squre-root
+void s_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qcqp_arg *cond_arg);
+
+//
+void s_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size);
+//
+void s_part_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim);
+//
+hpipm_size_t s_part_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg);
+//
+void s_part_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws, void *mem);
+//
+void s_part_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws);
+//
+void s_part_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws);
+//
+void s_part_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws);
+//
+void s_part_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_ocp_qcqp_sol *part_dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_D_PART_COND_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h b/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h
new file mode 100644
index 000000000..1a05ad1a2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h
@@ -0,0 +1,121 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_SIM_ERK_H_
+#define HPIPM_S_SIM_ERK_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct s_sim_erk_arg
+	{
+	struct s_sim_rk_data *rk_data; // integrator data
+	float h; // step size
+	int steps; // number of steps
+//	int for_sens; // compute adjoint sensitivities
+//	int adj_sens; // compute adjoint sensitivities
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_sim_erk_ws
+	{
+	void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to ode
+	void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to forward vde
+	void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out); // function pointer to adjoint vde
+	void *ode_args; // pointer to ode args
+	struct s_sim_erk_arg *erk_arg; // erk arg
+	float *K; // internal variables
+	float *x_for; // states and forward sensitivities
+	float *x_traj; // states at all steps
+	float *l; // adjoint sensitivities
+	float *p; // parameter
+	float *x_tmp; // temporary states and forward sensitivities
+	float *adj_in;
+	float *adj_tmp;
+	int nx; // number of states
+	int np; // number of parameters
+	int nf; // number of forward sensitivities
+	int na; // number of adjoint sensitivities
+	int nf_max; // max number of forward sensitivities
+	int na_max; // max number of adjoint sensitivities
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_sim_erk_arg_memsize();
+//
+void s_sim_erk_arg_create(struct s_sim_erk_arg *erk_arg, void *mem);
+//
+void s_sim_erk_arg_set_all(struct s_sim_rk_data *rk_data, float h, int steps, struct s_sim_erk_arg *erk_arg);
+
+//
+hpipm_size_t s_sim_erk_ws_memsize(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max);
+//
+void s_sim_erk_ws_create(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct s_sim_erk_ws *work, void *memory);
+//
+void s_sim_erk_ws_set_all(int nf, int na, float *x, float *fs, float *bs, float *p, void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out), void *ode_args, struct s_sim_erk_ws *work);
+// number of directions for forward sensitivities
+void s_sim_erk_ws_set_nf(int *nf, struct s_sim_erk_ws *work);
+// parameters (e.g. inputs)
+void s_sim_erk_ws_set_p(float *p, struct s_sim_erk_ws *work);
+// state
+void s_sim_erk_ws_set_x(float *x, struct s_sim_erk_ws *work);
+// forward sensitivities
+void s_sim_erk_ws_set_fs(float *fs, struct s_sim_erk_ws *work);
+// ode funtion
+void s_sim_erk_ws_set_ode(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work);
+// forward vde function
+void s_sim_erk_ws_set_vde_for(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work);
+// ode_args, passed straight to the ode/vde_for/vde_adj functions
+void s_sim_erk_ws_set_ode_args(void *ode_args, struct s_sim_erk_ws *work);
+// state
+void s_sim_erk_ws_get_x(struct s_sim_erk_ws *work, float *x);
+// forward sensitivities
+void s_sim_erk_ws_get_fs(struct s_sim_erk_ws *work, float *fs);
+//
+void s_sim_erk_solve(struct s_sim_erk_arg *arg, struct s_sim_erk_ws *work);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_D_SIM_ERK_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h b/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h
new file mode 100644
index 000000000..53acd7149
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h
@@ -0,0 +1,72 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_SIM_RK_H_
+#define HPIPM_S_SIM_RK_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct s_sim_rk_data
+	{
+	float *A_rk; // A in butcher tableau
+	float *B_rk; // b in butcher tableau
+	float *C_rk; // c in butcher tableau
+	int expl; // erk vs irk
+	int ns; // number of stages
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_sim_rk_data_memsize(int ns);
+//
+void s_sim_rk_data_create(int ns, struct s_sim_rk_data *rk_data, void *memory);
+//
+void s_sim_rk_data_init_default(char *field, struct s_sim_rk_data *rk_data);
+//
+void s_sim_rk_data_set_all(int expl, float *A_rk, float *B_rk, float *C_rk, struct s_sim_rk_data *rk_data);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_S_SIM_RK_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h
new file mode 100644
index 000000000..450e99262
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h
@@ -0,0 +1,213 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_H_
+#define HPIPM_S_TREE_OCP_QCQP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qcqp
+	{
+	struct s_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_smat *BAbt; // Nn-1
+	struct blasfeo_smat *RSQrq; // Nn
+	struct blasfeo_smat *DCt; // Nn
+	struct blasfeo_smat **Hq; // Nn
+	struct blasfeo_svec *b; // Nn-1
+	struct blasfeo_svec *rqz; // Nn
+	struct blasfeo_svec *d; // Nn
+	struct blasfeo_svec *d_mask; // Nn
+	struct blasfeo_svec *m; // Nn
+	struct blasfeo_svec *Z; // Nn
+	int **idxb; // indices of box constrained variables within [u; x] // Nn
+	int **idxs_rev; // index of soft constraints
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qcqp_memsize(struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp *qp, void *memory);
+//
+void s_tree_ocp_qcqp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_A(int edge, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_B(int edge, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_b(int edge, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Q(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_S(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_R(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_q(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_r(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lb(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ub(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lbx(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ubx(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lbu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ubu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_idxb(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jbx(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jbu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_C(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_D(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lg(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ug(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Qq(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Sq(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Rq(int node, float *mat, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_qq(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_rq(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_uq(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_uq_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Zl(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Zu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_zl(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_zu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lls(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lus(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_idxs(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jsg(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_set_Jsq(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_idxe(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_idxge(int node, int *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_Jbue(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_Jge(int node, float *vec, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qcqp *qp);
+
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h
new file mode 100644
index 000000000..7a49d5199
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h
@@ -0,0 +1,118 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_DIM_H_
+#define HPIPM_S_TREE_OCP_QCQP_DIM_H_
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qcqp_dim
+	{
+	struct s_tree_ocp_qp_dim *qp_dim; // dim of qp approximation
+	struct tree *ttree; // tree describing node conndection
+	int *nx; // number of states // Nn
+	int *nu; // number of inputs // Nn
+	int *nb; // number of box constraints // Nn
+	int *nbx; // number of state box constraints // Nn
+	int *nbu; // number of input box constraints // Nn
+	int *ng; // number of general constraints // Nn
+	int *nq; // number of (upper) quadratic constraints
+	int *ns; // number of soft constraints // Nn
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int *nsq; // number of (upper) soft quadratic constraints
+	int Nn; // number of nodes
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qcqp_dim_strsize();
+//
+hpipm_size_t s_tree_ocp_qcqp_dim_memsize(int Nn);
+//
+void s_tree_ocp_qcqp_dim_create(int Nn, struct s_tree_ocp_qcqp_dim *qp_dim, void *memory);
+//
+void s_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+//void s_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+//void s_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+//
+//void s_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct s_tree_ocp_qcqp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_DIM_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h
new file mode 100644
index 000000000..69d65914c
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h
@@ -0,0 +1,192 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_IPM_H_
+#define HPIPM_S_TREE_OCP_QCQP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_tree_ocp_qcqp_dim.h>
+#include <hpipm_s_tree_ocp_qp.h>
+#include <hpipm_s_tree_ocp_qcqp_res.h>
+#include <hpipm_s_tree_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qcqp_ipm_arg
+	{
+	struct s_tree_ocp_qp_ipm_arg *qp_arg;
+	float mu0; // initial value for complementarity slackness
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+//	int square_root_alg; // 0 classical Riccati, 1 square-root Riccati
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1)
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1)
+//	int comp_res_pred; // compute residuals of prediction
+	int split_step; // use different step for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_tree_ocp_qcqp_ipm_ws
+	{
+	struct s_tree_ocp_qp_ipm_ws *qp_ws;
+	struct s_tree_ocp_qp *qp;
+	struct s_tree_ocp_qp_sol *qp_sol;
+	struct s_tree_ocp_qcqp_res_ws *qcqp_res_ws;
+	struct s_tree_ocp_qcqp_res *qcqp_res;
+	struct blasfeo_svec *tmp_nuxM;
+	int iter; // iteration number
+	int status;
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qcqp_ipm_arg_strsize();
+//
+hpipm_size_t s_tree_ocp_qcqp_ipm_arg_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim);
+//
+void s_tree_ocp_qcqp_ipm_arg_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, void *mem);
+//
+void s_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qcqp_ipm_arg *arg);
+//
+void s_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set maximum number of iterations
+void s_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set minimum step lenght
+void s_tree_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set initial value of barrier parameter
+void s_tree_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on stationarity condition
+void s_tree_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on equality constr
+void s_tree_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on inequality constr
+void s_tree_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set exit tolerance on complementarity condition
+void s_tree_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set regularization of primal variables
+void s_tree_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set warm start: 0 no warm start, 1 primal var
+void s_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector
+void s_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector
+void s_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// set riccati algorithm: 0 classic, 1 square-root
+//void s_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// compute residuals after solution
+void s_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// compute residuals of prediction
+//void s_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// min value of lam in the solution
+void s_tree_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// min value of t in the solution
+void s_tree_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// use different step for primal and dual variables
+void s_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+void s_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg);
+
+//
+hpipm_size_t s_tree_ocp_qcqp_ipm_ws_strsize();
+//
+hpipm_size_t s_tree_ocp_qcqp_ipm_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg);
+//
+void s_tree_ocp_qcqp_ipm_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws, void *mem);
+//
+void s_tree_ocp_qcqp_ipm_get(char *field, struct s_tree_ocp_qcqp_ipm_ws *ws, void *value);
+//
+void s_tree_ocp_qcqp_ipm_get_status(struct s_tree_ocp_qcqp_ipm_ws *ws, int *status);
+//
+void s_tree_ocp_qcqp_ipm_get_iter(struct s_tree_ocp_qcqp_ipm_ws *ws, int *iter);
+//
+void s_tree_ocp_qcqp_ipm_get_max_res_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_stat);
+//
+void s_tree_ocp_qcqp_ipm_get_max_res_eq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_eq);
+//
+void s_tree_ocp_qcqp_ipm_get_max_res_ineq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_ineq);
+//
+void s_tree_ocp_qcqp_ipm_get_max_res_comp(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_comp);
+//
+void s_tree_ocp_qcqp_ipm_get_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float **stat);
+//
+void s_tree_ocp_qcqp_ipm_get_stat_m(struct s_tree_ocp_qcqp_ipm_ws *ws, int *stat_m);
+//
+void s_tree_ocp_qcqp_init_var(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws);
+//
+void s_tree_ocp_qcqp_ipm_solve(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_IPM_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h
new file mode 100644
index 000000000..d0d84a6f2
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h
@@ -0,0 +1,109 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_RES_H_
+#define HPIPM_S_TREE_OCP_QCQP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_tree_ocp_qcqp_dim.h>
+#include <hpipm_s_tree_ocp_qcqp.h>
+#include <hpipm_s_tree_ocp_qcqp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qcqp_res
+	{
+	struct s_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // max of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_tree_ocp_qcqp_res_ws
+	{
+	struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM
+	struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	struct blasfeo_svec *q_fun; // value for evaluation of quadr constr
+	struct blasfeo_svec *q_adj; // value for adjoint of quadr constr
+	int use_q_fun; // reuse cached value for evaluation of quadr constr
+	int use_q_adj; // reuse cached value for adjoint of quadr constr
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qcqp_res_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim);
+//
+void s_tree_ocp_qcqp_res_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res *res, void *mem);
+//
+hpipm_size_t s_tree_ocp_qcqp_res_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim);
+//
+void s_tree_ocp_qcqp_res_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res_ws *ws, void *mem);
+//
+void s_tree_ocp_qcqp_res_compute(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_res *res, struct s_tree_ocp_qcqp_res_ws *ws);
+//
+void s_tree_ocp_qcqp_res_compute_inf_norm(struct s_tree_ocp_qcqp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_RES_H_
+
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h
new file mode 100644
index 000000000..47f038c6b
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h
@@ -0,0 +1,97 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_SOL_H_
+#define HPIPM_S_TREE_OCP_QCQP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qcqp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct s_tree_ocp_qcqp_sol
+	{
+	struct s_tree_ocp_qcqp_dim *dim;
+	struct blasfeo_svec *ux;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qcqp_sol_memsize(struct s_tree_ocp_qcqp_dim *dim);
+//
+void s_tree_ocp_qcqp_sol_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp_sol *qp_sol, void *memory);
+//
+void s_tree_ocp_qcqp_sol_get_u(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_x(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_sl(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_su(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_pi(int edge, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_lam_lb(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_lam_ub(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_lam_lg(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qcqp_sol_get_lam_ug(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_SOL_H_
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h
new file mode 100644
index 000000000..79528de1c
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h
@@ -0,0 +1,85 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QCQP_UTILS_H_
+#define HPIPM_S_TREE_OCP_QCQP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qcqp_dim.h"
+#include "hpipm_s_tree_ocp_qcqp.h"
+#include "hpipm_s_tree_ocp_qcqp_sol.h"
+#include "hpipm_s_tree_ocp_qcqp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_tree_ocp_qcqp_dim_print(struct s_tree_ocp_qcqp_dim *qp_dim);
+//
+//void s_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim);
+//
+void s_tree_ocp_qcqp_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp);
+//
+//void s_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp);
+//
+void s_tree_ocp_qcqp_sol_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_sol *ocp_qcqp_sol);
+//
+void s_tree_ocp_qcqp_ipm_arg_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg);
+//
+//void s_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg);
+//
+void s_tree_ocp_qcqp_res_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_res *ocp_qcqp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QCQP_UTILS_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h
new file mode 100644
index 000000000..722b930b9
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h
@@ -0,0 +1,196 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QP_H_
+#define HPIPM_S_TREE_OCP_QP_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qp
+	{
+	struct s_tree_ocp_qp_dim *dim;
+	struct blasfeo_smat *BAbt; // Nn-1
+	struct blasfeo_smat *RSQrq; // Nn
+	struct blasfeo_smat *DCt; // Nn
+	struct blasfeo_svec *b; // Nn-1
+	struct blasfeo_svec *rqz; // Nn
+	struct blasfeo_svec *d; // Nn
+	struct blasfeo_svec *d_mask; // Nn
+	struct blasfeo_svec *m; // Nn
+	struct blasfeo_svec *Z; // Nn
+	int **idxb; // indices of box constrained variables within [u; x] // Nn
+//	int **idxs; // index of soft constraints
+	int **idxs_rev; // index of soft constraints
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qp_memsize(struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp *qp, void *memory);
+//
+void s_tree_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_A(int edge, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_B(int edge, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_b(int edge, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Q(int node, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_S(int node, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_R(int node, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_q(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_r(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lb(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ub(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lbx(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ubx(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lbu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ubu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_idxb(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_idxbx(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Jbx(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_idxbu(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Jbu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_C(int node, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_D(int node, float *mat, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lg(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ug(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Zl(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Zu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_zl(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_zu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lls(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lus(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_idxs(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_set_Jsg(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_idxe(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_idxbue(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_idxge(int node, int *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_Jbue(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_Jge(int node, float *vec, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qp *qp);
+
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QP_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h
new file mode 100644
index 000000000..90df57182
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h
@@ -0,0 +1,111 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QP_DIM_H_
+#define HPIPM_S_TREE_OCP_QP_DIM_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qp_dim
+	{
+	struct tree *ttree; // tree describing node conndection
+	int *nx; // number of states // Nn
+	int *nu; // number of inputs // Nn
+	int *nb; // number of box constraints // Nn
+	int *nbx; // number of state box constraints // Nn
+	int *nbu; // number of input box constraints // Nn
+	int *ng; // number of general constraints // Nn
+	int *ns; // number of soft constraints // Nn
+	int *nsbx; // number of soft state box constraints
+	int *nsbu; // number of soft input box constraints
+	int *nsg; // number of soft general constraints
+	int Nn; // number of nodes
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qp_dim_strsize();
+//
+hpipm_size_t s_tree_ocp_qp_dim_memsize(int Nn);
+//
+void s_tree_ocp_qp_dim_create(int Nn, struct s_tree_ocp_qp_dim *qp_dim, void *memory);
+//
+void s_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nx(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nu(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_ng(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_ns(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_dim_set_nge(int stage, int value, struct s_tree_ocp_qp_dim *dim);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QP_DIM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h
new file mode 100644
index 000000000..f8c26d317
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h
@@ -0,0 +1,208 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+
+#ifndef HPIPM_S_TREE_OCP_QP_IPM_H_
+#define HPIPM_S_TREE_OCP_QP_IPM_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_tree_ocp_qp_dim.h>
+#include <hpipm_s_tree_ocp_qp.h>
+#include <hpipm_s_tree_ocp_qp_res.h>
+#include <hpipm_s_tree_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qp_ipm_arg
+	{
+	float mu0; // initial value for duality measure
+	float alpha_min; // exit cond on step length
+	float res_g_max; // exit cond on inf norm of residuals
+	float res_b_max; // exit cond on inf norm of residuals
+	float res_d_max; // exit cond on inf norm of residuals
+	float res_m_max; // exit cond on inf norm of residuals
+	float reg_prim; // reg of primal hessian
+	float lam_min; // min value in lam vector
+	float t_min; // min value in t vector
+	float tau_min; // min value of barrier parameter
+	int iter_max; // exit cond in iter number
+	int stat_max; // iterations saved in stat
+	int stat_m; // number of recorded stat per IPM iter
+	int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm
+	int cond_pred_corr; // conditional Mehrotra's predictor-corrector
+	int itref_pred_max; // max number of iterative refinement steps for predictor step
+	int itref_corr_max; // max number of iterative refinement steps for corrector step
+	int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol
+	int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq
+	int abs_form; // absolute IPM formulation
+	int comp_dual_sol_eq; // dual solution (only for abs_form==1)
+	int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1)
+	int split_step; // use different steps for primal and dual variables
+	int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution
+	int mode;
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_tree_ocp_qp_ipm_ws
+	{
+	struct s_core_qp_ipm_workspace *core_workspace;
+	struct s_tree_ocp_qp_res_ws *res_workspace;
+	struct s_tree_ocp_qp_sol *sol_step;
+	struct s_tree_ocp_qp_sol *sol_itref;
+	struct s_tree_ocp_qp *qp_step;
+	struct s_tree_ocp_qp *qp_itref;
+	struct s_tree_ocp_qp_res *res_itref;
+	struct s_tree_ocp_qp_res *res;
+	struct blasfeo_svec *Gamma; // hessian update
+	struct blasfeo_svec *gamma; // hessian update
+	struct blasfeo_svec *tmp_nxM; // work space of size nxM
+	struct blasfeo_svec *tmp_nbgM; // work space of size nbgM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	struct blasfeo_svec *Pb; // Pb
+	struct blasfeo_svec *Zs_inv;
+	struct blasfeo_smat *L;
+	struct blasfeo_smat *Lh;
+	struct blasfeo_smat *AL;
+	struct blasfeo_smat *lq0;
+	struct blasfeo_svec *tmp_m;
+	float *stat; // convergence statistics
+	int *use_hess_fact;
+	void *lq_work0;
+	float qp_res[4]; // infinity norm of residuals
+	int iter; // iteration number
+	int stat_max; // iterations saved in stat
+	int stat_m; // number of recorded stat per IPM iter
+	int use_Pb;
+	int status; // solver status
+	int lq_fact; // cache from arg
+	int mask_constr; // use constr mask
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qp_ipm_arg_memsize(struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_ipm_arg_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, void *mem);
+//
+void s_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_reg_prim(float *reg, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qp_ipm_arg *arg);
+
+//
+hpipm_size_t s_tree_ocp_qp_ipm_ws_memsize(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_ipm_ws_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws, void *mem);
+//
+void s_tree_ocp_qp_ipm_get_status(struct s_tree_ocp_qp_ipm_ws *ws, int *status);
+//
+void s_tree_ocp_qp_ipm_get_iter(struct s_tree_ocp_qp_ipm_ws *ws, int *iter);
+//
+void s_tree_ocp_qp_ipm_get_max_res_stat(struct s_tree_ocp_qp_ipm_ws *ws, float *res_stat);
+//
+void s_tree_ocp_qp_ipm_get_max_res_eq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_eq);
+//
+void s_tree_ocp_qp_ipm_get_max_res_ineq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_ineq);
+//
+void s_tree_ocp_qp_ipm_get_max_res_comp(struct s_tree_ocp_qp_ipm_ws *ws, float *res_comp);
+//
+void s_tree_ocp_qp_ipm_get_stat(struct s_tree_ocp_qp_ipm_ws *ws, float **stat);
+//
+void s_tree_ocp_qp_ipm_get_stat_m(struct s_tree_ocp_qp_ipm_ws *ws, int *stat_m);
+//
+void s_tree_ocp_qp_init_var(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_ipm_abs_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_ipm_delta_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_ipm_solve(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QP_IPM_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h
new file mode 100644
index 000000000..bcf735478
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h
@@ -0,0 +1,54 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//
+void s_tree_ocp_qp_fact_solve_kkt_unconstr(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_fact_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_fact_lq_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+//
+void s_tree_ocp_qp_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h
new file mode 100644
index 000000000..5c6797204
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h
@@ -0,0 +1,107 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QP_RES_H_
+#define HPIPM_S_TREE_OCP_QP_RES_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include <hpipm_common.h>
+#include <hpipm_s_tree_ocp_qp_dim.h>
+#include <hpipm_s_tree_ocp_qp.h>
+#include <hpipm_s_tree_ocp_qp_sol.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct s_tree_ocp_qp_res
+	{
+	struct s_tree_ocp_qp_dim *dim;
+	struct blasfeo_svec *res_g; // q-residuals
+	struct blasfeo_svec *res_b; // b-residuals
+	struct blasfeo_svec *res_d; // d-residuals
+	struct blasfeo_svec *res_m; // m-residuals
+	float res_max[4]; // max of residuals
+	float res_mu; // mu-residual
+	hpipm_size_t memsize;
+	};
+
+
+
+struct s_tree_ocp_qp_res_ws
+	{
+	struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_svec *tmp_nsM; // work space of size nsM
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qp_res_memsize(struct s_tree_ocp_qp_dim *ocp_dim);
+//
+void s_tree_ocp_qp_res_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res *res, void *mem);
+//
+hpipm_size_t s_tree_ocp_qp_res_ws_memsize(struct s_tree_ocp_qp_dim *ocp_dim);
+//
+void s_tree_ocp_qp_res_ws_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res_ws *ws, void *mem);
+//
+void s_tree_ocp_qp_res_get_all(struct s_tree_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us);
+//
+void s_tree_ocp_qp_res_compute(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws);
+//
+void s_tree_ocp_qp_res_compute_lin(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_sol *qp_step, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws);
+//
+void s_tree_ocp_qp_res_compute_inf_norm(struct s_tree_ocp_qp_res *res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_S_TREE_OCP_QP_RES_H_
+
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h
new file mode 100644
index 000000000..71e887675
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h
@@ -0,0 +1,98 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QP_SOL_H_
+#define HPIPM_S_TREE_OCP_QP_SOL_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qp_dim.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct s_tree_ocp_qp_sol
+	{
+	struct s_tree_ocp_qp_dim *dim;
+	struct blasfeo_svec *ux;
+	struct blasfeo_svec *pi;
+	struct blasfeo_svec *lam;
+	struct blasfeo_svec *t;
+	void *misc;
+	hpipm_size_t memsize; // memory size in bytes
+	};
+
+
+
+//
+hpipm_size_t s_tree_ocp_qp_sol_memsize(struct s_tree_ocp_qp_dim *dim);
+//
+void s_tree_ocp_qp_sol_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_sol *qp_sol, void *memory);
+//
+void s_tree_ocp_qp_sol_get_all(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us);
+//
+void s_tree_ocp_qp_sol_get_u(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_x(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_sl(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_su(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_pi(int edge, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_lam_lb(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_lam_ub(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_lam_lg(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+//
+void s_tree_ocp_qp_sol_get_lam_ug(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QP_SOL_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h
new file mode 100644
index 000000000..ec1747568
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h
@@ -0,0 +1,84 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_S_TREE_OCP_QP_UTILS_H_
+#define HPIPM_S_TREE_OCP_QP_UTILS_H_
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+#include "hpipm_s_tree_ocp_qp_dim.h"
+#include "hpipm_s_tree_ocp_qp.h"
+#include "hpipm_s_tree_ocp_qp_sol.h"
+#include "hpipm_s_tree_ocp_qp_ipm.h"
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+//
+void s_tree_ocp_qp_dim_print(struct s_tree_ocp_qp_dim *qp_dim);
+//
+//void s_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim);
+//
+void s_tree_ocp_qp_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp);
+//
+//void s_tree_ocp_qp_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp);
+//
+void s_tree_ocp_qp_sol_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_sol *ocp_qp_sol);
+//
+void s_tree_ocp_qp_ipm_arg_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+//void s_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg);
+//
+void s_tree_ocp_qp_res_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_res *ocp_qp_res);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+
+#endif // HPIPM_S_TREE_OCP_QP_UTILS_H_
+
+
diff --git a/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h b/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h
new file mode 100644
index 000000000..a3c77a670
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h
@@ -0,0 +1,70 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_SCENARIO_TREE_H_
+#define HPIPM_SCENARIO_TREE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct sctree
+	{
+	struct node *root; // pointer to root
+	int *kids; // pointer to array of kids
+	int Nn; // numer of nodes
+	int md; // number of realizations
+	int Nr; // robust horizion
+	int Nh; // control horizion
+	hpipm_size_t memsize;
+	};
+
+
+
+//
+hpipm_size_t sctree_memsize(int md, int Nr, int Nh);
+//
+void sctree_create(int md, int Nr, int Nh, struct sctree *st, void *memory);
+//
+void sctree_cast_to_tree(struct sctree *st, struct tree *tt);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_SCENARIO_TREE_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_timing.h b/third_party/acados/include/hpipm/include/hpipm_timing.h
new file mode 100644
index 000000000..bd0f2dbcb
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_timing.h
@@ -0,0 +1,67 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+#ifndef HPIPM_TIMING_H_
+#define HPIPM_TIMING_H_
+
+
+
+#include <blasfeo_timing.h>
+
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+typedef blasfeo_timer hpipm_timer;
+
+
+
+//
+void hpipm_tic(hpipm_timer *t);
+//
+double hpipm_toc(hpipm_timer *t);
+
+
+
+#ifdef __cplusplus
+}	// #extern "C"
+#endif
+
+
+#endif // HPIPM_TIMING_H_
diff --git a/third_party/acados/include/hpipm/include/hpipm_tree.h b/third_party/acados/include/hpipm/include/hpipm_tree.h
new file mode 100644
index 000000000..f9d69ecdd
--- /dev/null
+++ b/third_party/acados/include/hpipm/include/hpipm_tree.h
@@ -0,0 +1,76 @@
+/**************************************************************************************************
+*                                                                                                 *
+* This file is part of HPIPM.                                                                     *
+*                                                                                                 *
+* HPIPM -- High-Performance Interior Point Method.                                                *
+* Copyright (C) 2019 by Gianluca Frison.                                                          *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl.              *
+* All rights reserved.                                                                            *
+*                                                                                                 *
+* The 2-Clause BSD License                                                                        *
+*                                                                                                 *
+* Redistribution and use in source and binary forms, with or without                              *
+* modification, are permitted provided that the following conditions are met:                     *
+*                                                                                                 *
+* 1. Redistributions of source code must retain the above copyright notice, this                  *
+*    list of conditions and the following disclaimer.                                             *
+* 2. Redistributions in binary form must reproduce the above copyright notice,                    *
+*    this list of conditions and the following disclaimer in the documentation                    *
+*    and/or other materials provided with the distribution.                                       *
+*                                                                                                 *
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND                 *
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED                   *
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE                          *
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR                 *
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES                  *
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;                    *
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND                     *
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                      *
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS                   *
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.                                    *
+*                                                                                                 *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de                             *
+*                                                                                                 *
+**************************************************************************************************/
+
+
+#ifndef HPIPM_TREE_H_
+#define HPIPM_TREE_H_
+
+#include "hpipm_common.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+struct node
+	{
+	int *kids;  // 64 bits
+	int idx;    // 32 bits
+	int dad;    // 32 bits
+	int nkids;  // 32 bits
+	int stage;  // 32 bits
+	int real;   // 32 bits
+	int idxkid; // 32 bits // XXX needed ???
+	// total     256 bits
+	};
+
+
+
+struct tree
+	{
+	struct node *root; // pointer to root
+	int *kids; // pointer to array of kids
+	int Nn; // numer of nodes
+	hpipm_size_t memsize;
+	};
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif // HPIPM_TREE_H_
diff --git a/third_party/acados/include/qpOASES_e/Bounds.h b/third_party/acados/include/qpOASES_e/Bounds.h
new file mode 100644
index 000000000..4e41c1d16
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Bounds.h
@@ -0,0 +1,543 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Bounds.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Bounds class designed to manage working sets of
+ *	bounds within a QProblem.
+ */
+
+
+#ifndef QPOASES_BOUNDS_H
+#define QPOASES_BOUNDS_H
+
+
+#include <qpOASES_e/Indexlist.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Manages working sets of bounds (= box constraints).
+ *
+ *	This class manages working sets of bounds (= box constraints)
+ *	by storing index sets and other status information.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	Indexlist *freee;					/**< Index list of free variables. */
+	Indexlist *fixed;					/**< Index list of fixed variables. */
+
+	Indexlist *shiftedFreee;			/**< Memory for shifting free variables. */
+	Indexlist *shiftedFixed;			/**< Memory for shifting fixed variables. */
+
+	Indexlist *rotatedFreee;			/**< Memory for rotating free variables. */
+	Indexlist *rotatedFixed;			/**< Memory for rotating fixed variables. */
+
+	SubjectToType   *type; 				/**< Type of bounds. */
+	SubjectToStatus *status;			/**< Status of bounds. */
+
+	SubjectToType   *typeTmp;			/**< Temp memory for type of bounds. */
+	SubjectToStatus *statusTmp;			/**< Temp memory for status of bounds. */
+
+	BooleanType noLower;	 			/**< This flag indicates if there is no lower bound on any variable. */
+	BooleanType noUpper;	 			/**< This flag indicates if there is no upper bound on any variable. */
+
+	int n;								/**< Total number of bounds. */
+} Bounds;
+
+int Bounds_calculateMemorySize( int n);
+
+char *Bounds_assignMemory(int n, Bounds **mem, void *raw_memory);
+
+Bounds *Bounds_createMemory( int n );
+
+/** Constructor which takes the number of bounds. */
+void BoundsCON(	Bounds* _THIS,
+				int _n									/**< Number of bounds. */
+				);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void BoundsCPY(	Bounds* FROM,
+				Bounds* TO
+				);
+
+
+/** Initialises object with given number of bounds.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INVALID_ARGUMENTS */
+returnValue Bounds_init(	Bounds* _THIS,
+							int _n					/**< Number of bounds. */
+							);
+
+
+/** Initially adds number of a new (i.e. not yet in the list) bound to
+ *  given index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_BOUND_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS \n
+			RET_INVALID_ARGUMENTS */
+returnValue Bounds_setupBound(	Bounds* _THIS,
+								int number,				/**< Number of new bound. */
+								SubjectToStatus _status	/**< Status of new bound. */
+								);
+
+/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+ *  to the index set of free bounds; the order depends on the SujectToType
+ *  of each index.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_BOUND_FAILED */
+returnValue Bounds_setupAllFree(	Bounds* _THIS
+									);
+
+/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+ *  to the index set of fixed bounds (on their lower bounds);
+ *  the order depends on the SujectToType of each index.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_BOUND_FAILED */
+returnValue Bounds_setupAllLower(	Bounds* _THIS
+									);
+
+/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+ *  to the index set of fixed bounds (on their upper bounds);
+ *  the order depends on the SujectToType of each index.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_BOUND_FAILED */
+returnValue Bounds_setupAllUpper(	Bounds* _THIS
+									);
+
+
+/** Moves index of a bound from index list of fixed to that of free bounds.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_BOUND_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS */
+returnValue Bounds_moveFixedToFree(	Bounds* _THIS,
+									int number				/**< Number of bound to be freed. */
+									);
+
+/** Moves index of a bound from index list of free to that of fixed bounds.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_BOUND_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS */
+returnValue Bounds_moveFreeToFixed(	Bounds* _THIS,
+									int number,				/**< Number of bound to be fixed. */
+									SubjectToStatus _status	/**< Status of bound to be fixed. */
+									);
+
+/** Flip fixed bound.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_BOUND_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS */
+returnValue Bounds_flipFixed(	Bounds* _THIS,
+								int number
+								);
+
+/** Swaps the indices of two free bounds within the index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SWAPINDEX_FAILED */
+returnValue Bounds_swapFree(	Bounds* _THIS,
+								int number1,					/**< Number of first bound. */
+								int number2						/**< Number of second bound. */
+								);
+
+
+/** Returns number of variables.
+ *	\return Number of variables. */
+static inline int Bounds_getNV(	Bounds* _THIS
+							);
+
+/** Returns number of implicitly fixed variables.
+ *	\return Number of implicitly fixed variables. */
+static inline int Bounds_getNFV(	Bounds* _THIS
+									);
+
+/** Returns number of bounded (but possibly free) variables.
+ *	\return Number of bounded (but possibly free) variables. */
+static inline int Bounds_getNBV(	Bounds* _THIS
+									);
+
+/** Returns number of unbounded variables.
+ *	\return Number of unbounded variables. */
+static inline int Bounds_getNUV(	Bounds* _THIS
+									);
+
+/** Returns number of free variables.
+ *	\return Number of free variables. */
+static inline int Bounds_getNFR(	Bounds* _THIS
+									);
+
+/** Returns number of fixed variables.
+ *	\return Number of fixed variables. */
+static inline int Bounds_getNFX(	Bounds* _THIS
+									);
+
+
+/** Returns a pointer to free variables index list.
+ *	\return Pointer to free variables index list. */
+static inline Indexlist* Bounds_getFree(	Bounds* _THIS
+											);
+
+/** Returns a pointer to fixed variables index list.
+ *	\return Pointer to fixed variables index list. */
+static inline Indexlist* Bounds_getFixed(	Bounds* _THIS
+											);
+
+
+/** Returns number of bounds with given SubjectTo type.
+ *	\return Number of bounds with given type. */
+static inline int Bounds_getNumberOfType(	Bounds* _THIS,
+											SubjectToType _type	/**< Type of bound. */
+											);
+
+
+/** Returns type of bound.
+ *	\return Type of bound \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline SubjectToType Bounds_getType(	Bounds* _THIS,
+											int i			/**< Number of bound. */
+											);
+
+/** Returns status of bound.
+ *	\return Status of bound \n
+ 			ST_UNDEFINED */
+static inline SubjectToStatus Bounds_getStatus(	Bounds* _THIS,
+												int i		/**< Number of bound. */
+												);
+
+
+/** Sets type of bound.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue Bounds_setType(	Bounds* _THIS,
+											int i,				/**< Number of bound. */
+											SubjectToType value	/**< Type of bound. */
+											);
+
+/** Sets status of bound.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue Bounds_setStatus(	Bounds* _THIS,
+											int i,					/**< Number of bound. */
+											SubjectToStatus value	/**< Status of bound. */
+											);
+
+
+/** Sets status of lower bounds. */
+static inline void Bounds_setNoLower(	Bounds* _THIS,
+										BooleanType _status		/**< Status of lower bounds. */
+										);
+
+/** Sets status of upper bounds. */
+static inline void Bounds_setNoUpper(	Bounds* _THIS,
+										BooleanType _status		/**< Status of upper bounds. */
+										);
+
+
+/** Returns status of lower bounds.
+ *	\return BT_TRUE if there is no lower bound on any variable. */
+static inline BooleanType Bounds_hasNoLower(	Bounds* _THIS
+												);
+
+/** Returns status of upper bounds.
+ *	\return BT_TRUE if there is no upper bound on any variable. */
+static inline BooleanType Bounds_hasNoUpper(	Bounds* _THIS
+												);
+
+
+/** Shifts forward type and status of all bounds by a given
+ *  offset. This offset has to lie within the range [0,n/2] and has to
+ *  be an integer divisor of the total number of bounds n.
+ *  Type and status of the first \<offset\> bounds is thrown away,
+ *  type and status of the last \<offset\> bounds is doubled,
+ *  e.g. for offset = 2: \n
+ *  shift( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b5,b6}
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS \n
+ 			RET_INVALID_ARGUMENTS \n
+ 			RET_SHIFTING_FAILED */
+returnValue Bounds_shift(	Bounds* _THIS,
+							int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
+							);
+
+/** Rotates forward type and status of all bounds by a given
+ *  offset. This offset has to lie within the range [0,n].
+ *  Example for offset = 2: \n
+ *  rotate( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b1,b2}
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS \n
+ 			RET_ROTATING_FAILED */
+returnValue Bounds_rotate(	Bounds* _THIS,
+							int offset		/**< Rotation offset within the range [0,n]. */
+							);
+
+
+/** Prints information on bounds object
+ *  (in particular, lists of free and fixed bounds.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INDEXLIST_CORRUPTED */
+returnValue Bounds_print(	Bounds* _THIS
+							);
+
+
+/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+ *  to the index set corresponding to the desired status;
+ *  the order depends on the SujectToType of each index.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_BOUND_FAILED */
+returnValue Bounds_setupAll(	Bounds* _THIS,
+								SubjectToStatus _status	/**< Desired initial status for all bounds. */
+								);
+
+
+/** Adds the index of a new bound to index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Bounds_addIndex(	Bounds* _THIS,
+								Indexlist* const indexlist,	/**< Index list to which the new index shall be added. */
+								int newnumber,				/**< Number of new bound. */
+								SubjectToStatus newstatus	/**< Status of new bound. */
+								);
+
+/** Removes the index of a bound from index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_REMOVEINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Bounds_removeIndex(	Bounds* _THIS,
+								Indexlist* const indexlist,	/**< Index list from which the new index shall be removed. */
+								int removenumber			/**< Number of bound to be removed. */
+								);
+
+/** Swaps the indices of two constraints or bounds within the index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SWAPINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Bounds_swapIndex(	Bounds* _THIS,
+								Indexlist* const indexlist,	/**< Index list in which the indices shold be swapped. */
+								int number1,				/**< Number of first bound. */
+								int number2					/**< Number of second bound. */
+								);
+
+
+
+/*
+ *	g e t N u m b e r O f T y p e
+ */
+static inline int Bounds_getNumberOfType( Bounds* _THIS, SubjectToType _type )
+{
+	int i;
+	int numberOfType = 0;
+
+	if ( _THIS->type != 0 )
+	{
+		for( i=0; i<_THIS->n; ++i )
+			if ( _THIS->type[i] == _type )
+				++numberOfType;
+	}
+
+	return numberOfType;
+}
+
+
+/*
+ *	g e t T y p e
+ */
+static inline SubjectToType Bounds_getType( Bounds* _THIS, int i )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+		return _THIS->type[i];
+
+	return ST_UNKNOWN;
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+static inline SubjectToStatus Bounds_getStatus( Bounds* _THIS, int i )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+		return _THIS->status[i];
+
+	return ST_UNDEFINED;
+}
+
+
+/*
+ *	s e t T y p e
+ */
+static inline returnValue Bounds_setType( Bounds* _THIS, int i, SubjectToType value )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+	{
+		_THIS->type[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t S t a t u s
+ */
+static inline returnValue Bounds_setStatus( Bounds* _THIS, int i, SubjectToStatus value )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+	{
+		_THIS->status[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t N o L o w e r
+ */
+static inline void Bounds_setNoLower( Bounds* _THIS, BooleanType _status )
+{
+	_THIS->noLower = _status;
+}
+
+
+/*
+ *	s e t N o U p p e r
+ */
+static inline void Bounds_setNoUpper( Bounds* _THIS, BooleanType _status )
+{
+	_THIS->noUpper = _status;
+}
+
+
+/*
+ *	h a s N o L o w e r
+ */
+static inline BooleanType Bounds_hasNoLower( Bounds* _THIS )
+{
+	return _THIS->noLower;
+}
+
+
+/*
+ *	h a s N o U p p p e r
+ */
+static inline BooleanType Bounds_hasNoUpper( Bounds* _THIS )
+{
+	return _THIS->noUpper;
+}
+
+
+
+/*
+ *	g e t N V
+ */
+static inline int Bounds_getNV( Bounds* _THIS )
+{
+ 	return _THIS->n;
+}
+
+
+/*
+ *	g e t N F V
+ */
+static inline int Bounds_getNFV( Bounds* _THIS )
+{
+ 	return Bounds_getNumberOfType( _THIS,ST_EQUALITY );
+}
+
+
+/*
+ *	g e t N B V
+ */
+static inline int Bounds_getNBV( Bounds* _THIS )
+{
+ 	return Bounds_getNumberOfType( _THIS,ST_BOUNDED );
+}
+
+
+/*
+ *	g e t N U V
+ */
+static inline int Bounds_getNUV( Bounds* _THIS )
+{
+	return Bounds_getNumberOfType( _THIS,ST_UNBOUNDED );
+}
+
+
+/*
+ *	g e t N F R
+ */
+static inline int Bounds_getNFR( Bounds* _THIS )
+{
+ 	return Indexlist_getLength( _THIS->freee );
+}
+
+
+/*
+ *	g e t N F X
+ */
+static inline int Bounds_getNFX( Bounds* _THIS )
+{
+	return Indexlist_getLength( _THIS->fixed );
+}
+
+
+/*
+ *	g e t F r e e
+ */
+static inline Indexlist* Bounds_getFree( Bounds* _THIS )
+{
+	return _THIS->freee;
+}
+
+
+/*
+ *	g e t F i x e d
+ */
+static inline Indexlist* Bounds_getFixed( Bounds* _THIS )
+{
+	return _THIS->fixed;
+}
+
+
+END_NAMESPACE_QPOASES
+
+#endif	/* QPOASES_BOUNDS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Constants.h b/third_party/acados/include/qpOASES_e/Constants.h
new file mode 100644
index 000000000..0e3dcd19f
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Constants.h
@@ -0,0 +1,134 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Constants.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Definition of all global constants.
+ */
+
+
+#ifndef QPOASES_CONSTANTS_H
+#define QPOASES_CONSTANTS_H
+
+
+#include <qpOASES_e/Types.h>
+
+#ifdef __CODE_GENERATION__
+
+  #define CONVERTTOSTRINGAUX(x) #x
+  #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x)
+
+  #ifndef QPOASES_CUSTOM_INTERFACE
+  #include "acado_qpoases3_interface.h"
+  #else
+  #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE)
+  #endif
+
+#endif
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+#ifndef __EXTERNAL_DIMENSIONS__
+
+  /*#define QPOASES_NVMAX  50
+  #define QPOASES_NCMAX  100*/
+  #define QPOASES_NVMAX  287
+  #define QPOASES_NCMAX  709
+
+#endif /* __EXTERNAL_DIMENSIONS__ */
+
+
+/** Maximum number of variables within a QP formulation.
+ *	Note: this value has to be positive! */
+#define NVMAX  QPOASES_NVMAX
+
+/** Maximum number of constraints within a QP formulation.
+ *	Note: this value has to be positive! */
+#define NCMAX  QPOASES_NCMAX
+
+#if ( QPOASES_NVMAX > QPOASES_NCMAX )
+#define NVCMAX QPOASES_NVMAX
+#else
+#define NVCMAX QPOASES_NCMAX
+#endif
+
+#if ( QPOASES_NVMAX > QPOASES_NCMAX )
+#define NVCMIN QPOASES_NCMAX
+#else
+#define NVCMIN QPOASES_NVMAX
+#endif
+
+
+/** Maximum number of QPs in a sequence solved by means of the OQP interface.
+ *	Note: this value has to be positive! */
+#define NQPMAX 1000
+
+
+/** Numerical value of machine precision (min eps, s.t. 1+eps > 1).
+ *	Note: this value has to be positive! */
+#ifndef __CODE_GENERATION__
+
+  #ifdef __USE_SINGLE_PRECISION__
+  static const real_t QPOASES_EPS = 1.193e-07;
+  #else
+  static const real_t QPOASES_EPS = 2.221e-16;
+  #endif /* __USE_SINGLE_PRECISION__ */
+
+#endif /* __CODE_GENERATION__ */
+
+
+/** Numerical value of zero (for situations in which it would be
+ *	unreasonable to compare with 0.0).
+ *  Note: this value has to be positive! */
+static const real_t QPOASES_ZERO = 1.0e-25;
+
+/** Numerical value of infinity (e.g. for non-existing bounds).
+ *	Note: this value has to be positive! */
+static const real_t QPOASES_INFTY = 1.0e20;
+
+/** Tolerance to used for isEqual, isZero etc.
+ *	Note: this value has to be positive! */
+static const real_t QPOASES_TOL = 1.0e-25;
+
+
+/** Maximum number of characters within a string.
+ *	Note: this value should be at least 41! */
+#define QPOASES_MAX_STRING_LENGTH 160
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_CONSTANTS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/ConstraintProduct.h b/third_party/acados/include/qpOASES_e/ConstraintProduct.h
new file mode 100644
index 000000000..ddfcfbe5d
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/ConstraintProduct.h
@@ -0,0 +1,62 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/ConstraintProduct.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor)
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ *
+ *	Declaration of the ConstraintProduct interface which allows to specify a
+ *	user-defined function for evaluating the constraint product at the 
+ *	current iterate to speed-up QP solution in case of a specially structured
+ *	constraint matrix.
+ */
+
+
+
+#ifndef QPOASES_CONSTRAINT_PRODUCT_H
+#define QPOASES_CONSTRAINT_PRODUCT_H
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** 
+ *	\brief Interface for specifying user-defined evaluations of constraint products.
+ *
+ *	An interface which allows to specify a user-defined function for evaluating the 
+ *	constraint product at the current iterate to speed-up QP solution in case 
+ *	of a specially structured constraint matrix.
+ *
+ *	\author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor)
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ */
+typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const );
+
+
+END_NAMESPACE_QPOASES
+
+#endif	/* QPOASES_CONSTRAINT_PRODUCT_H */
diff --git a/third_party/acados/include/qpOASES_e/Constraints.h b/third_party/acados/include/qpOASES_e/Constraints.h
new file mode 100644
index 000000000..8aca10d5d
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Constraints.h
@@ -0,0 +1,535 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Constraints.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Constraints class designed to manage working sets of
+ *	constraints within a QProblem.
+ */
+
+
+#ifndef QPOASES_CONSTRAINTS_H
+#define QPOASES_CONSTRAINTS_H
+
+
+#include <qpOASES_e/Indexlist.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Manages working sets of constraints.
+ *
+ *	This class manages working sets of constraints by storing
+ *	index sets and other status information.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	Indexlist *active;					/**< Index list of active constraints. */
+	Indexlist *inactive;				/**< Index list of inactive constraints. */
+
+	Indexlist *shiftedActive;			/**< Memory for shifting active constraints. */
+	Indexlist *shiftedInactive;			/**< Memory for shifting inactive constraints. */
+
+	Indexlist *rotatedActive;			/**< Memory for rotating active constraints. */
+	Indexlist *rotatedInactive;			/**< Memory for rotating inactive constraints. */
+
+	SubjectToType   *type; 				/**< Type of constraints. */
+	SubjectToStatus *status;			/**< Status of constraints. */
+
+	SubjectToType   *typeTmp;			/**< Temp memory for type of constraints. */
+	SubjectToStatus *statusTmp;			/**< Temp memory for status of constraints. */
+
+	BooleanType noLower;			 	/**< This flag indicates if there is no lower bound on any variable. */
+	BooleanType noUpper;	 			/**< This flag indicates if there is no upper bound on any variable. */
+
+	int n;								/**< Total number of constraints. */
+} Constraints;
+
+int Constraints_calculateMemorySize( int n);
+
+char *Constraints_assignMemory(int n, Constraints **mem, void *raw_memory);
+
+Constraints *Constraints_createMemory( int n );
+
+/** Constructor which takes the number of constraints. */
+void ConstraintsCON(	Constraints* _THIS,
+						int _n							/**< Number of constraints. */
+						);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void ConstraintsCPY(	Constraints* FROM,
+						Constraints* TO
+						);
+
+
+/** Initialises object with given number of constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INVALID_ARGUMENTS */
+returnValue Constraints_init(	Constraints* _THIS,
+								int _n					/**< Number of constraints. */
+								);
+
+
+/** Initially adds number of a new (i.e. not yet in the list) constraint to
+ *  a given index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_CONSTRAINT_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS \n
+			RET_INVALID_ARGUMENTS */
+returnValue Constraints_setupConstraint(	Constraints* _THIS,
+											int number,				/**< Number of new constraint. */
+											SubjectToStatus _status	/**< Status of new constraint. */
+											);
+
+/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+ *  to the index set of inactive constraints; the order depends on the SujectToType
+ *  of each index. Only disabled constraints are added to index set of disabled constraints!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_CONSTRAINT_FAILED */
+returnValue Constraints_setupAllInactive(	Constraints* _THIS
+											);
+
+/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+ *  to the index set of active constraints (on their lower bounds); the order depends on the SujectToType
+ *  of each index. Only disabled constraints are added to index set of disabled constraints!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_CONSTRAINT_FAILED */
+returnValue Constraints_setupAllLower(	Constraints* _THIS
+										);
+
+/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to
+ *  to the index set of active constraints (on their upper bounds); the order depends on the SujectToType
+ *  of each index. Only disabled constraints are added to index set of disabled constraints!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_CONSTRAINT_FAILED */
+returnValue Constraints_setupAllUpper(	Constraints* _THIS
+										);
+
+
+/** Moves index of a constraint from index list of active to that of inactive constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_CONSTRAINT_FAILED */
+returnValue Constraints_moveActiveToInactive(	Constraints* _THIS,
+												int number				/**< Number of constraint to become inactive. */
+												);
+
+/** Moves index of a constraint from index list of inactive to that of active constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_CONSTRAINT_FAILED */
+returnValue Constraints_moveInactiveToActive(	Constraints* _THIS,
+												int number,				/**< Number of constraint to become active. */
+												SubjectToStatus _status	/**< Status of constraint to become active. */
+												);
+
+/** Flip fixed constraint.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MOVING_CONSTRAINT_FAILED \n
+			RET_INDEX_OUT_OF_BOUNDS */
+returnValue Constraints_flipFixed(	Constraints* _THIS,
+									int number
+									);
+
+
+/** Returns the number of constraints.
+ *	\return Number of constraints. */
+static inline int Constraints_getNC(	Constraints* _THIS
+										);
+
+/** Returns the number of implicit equality constraints.
+ *	\return Number of implicit equality constraints. */
+static inline int Constraints_getNEC(	Constraints* _THIS
+										);
+
+/** Returns the number of "real" inequality constraints.
+ *	\return Number of "real" inequality constraints. */
+static inline int Constraints_getNIC(	Constraints* _THIS
+										);
+
+/** Returns the number of unbounded constraints (i.e. without any bounds).
+ *	\return Number of unbounded constraints (i.e. without any bounds). */
+static inline int Constraints_getNUC(	Constraints* _THIS
+										);
+
+/** Returns the number of active constraints.
+ *	\return Number of active constraints. */
+static inline int Constraints_getNAC(	Constraints* _THIS
+										);
+
+/** Returns the number of inactive constraints.
+ *	\return Number of inactive constraints. */
+static inline int Constraints_getNIAC(	Constraints* _THIS
+										);
+
+
+/** Returns a pointer to active constraints index list.
+ *	\return Pointer to active constraints index list. */
+static inline Indexlist* Constraints_getActive(	Constraints* _THIS
+												);
+
+/** Returns a pointer to inactive constraints index list.
+ *	\return Pointer to inactive constraints index list. */
+static inline Indexlist* Constraints_getInactive(	Constraints* _THIS
+													);
+
+
+/** Returns number of constraints with given SubjectTo type.
+ *	\return Number of constraints with given type. */
+static inline int Constraints_getNumberOfType(	Constraints* _THIS,
+												SubjectToType _type	/**< Type of constraints' bound. */
+												);
+
+
+/** Returns type of constraints' bound.
+ *	\return Type of constraints' bound \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline SubjectToType Constraints_getType(	Constraints* _THIS,
+													int i			/**< Number of constraints' bound. */
+													);
+
+/** Returns status of constraints' bound.
+ *	\return Status of constraints' bound \n
+ 			ST_UNDEFINED */
+static inline SubjectToStatus Constraints_getStatus(	Constraints* _THIS,
+														int i		/**< Number of constraints' bound. */
+														);
+
+
+/** Sets type of constraints' bound.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue Constraints_setType(	Constraints* _THIS,
+												int i,				/**< Number of constraints' bound. */
+												SubjectToType value	/**< Type of constraints' bound. */
+												);
+
+/** Sets status of constraints' bound.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue Constraints_setStatus(	Constraints* _THIS,
+													int i,					/**< Number of constraints' bound. */
+													SubjectToStatus value	/**< Status of constraints' bound. */
+													);
+
+
+/** Sets status of lower constraints' bounds. */
+static inline void Constraints_setNoLower(	Constraints* _THIS,
+											BooleanType _status		/**< Status of lower constraints' bounds. */
+											);
+
+/** Sets status of upper constraints' bounds. */
+static inline void Constraints_setNoUpper(	Constraints* _THIS,
+											BooleanType _status		/**< Status of upper constraints' bounds. */
+											);
+
+
+/** Returns status of lower constraints' bounds.
+ *	\return BT_TRUE if there is no lower constraints' bound on any variable. */
+static inline BooleanType Constraints_hasNoLower(	Constraints* _THIS
+													);
+
+/** Returns status of upper bounds.
+ *	\return BT_TRUE if there is no upper constraints' bound on any variable. */
+static inline BooleanType Constraints_hasNoUpper(	Constraints* _THIS
+													);
+
+
+/** Shifts forward type and status of all constraints by a given
+ *  offset. This offset has to lie within the range [0,n/2] and has to
+ *  be an integer divisor of the total number of constraints n.
+ *  Type and status of the first \<offset\> constraints  is thrown away,
+ *  type and status of the last \<offset\> constraints is doubled,
+ *  e.g. for offset = 2: \n
+ *  shift( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b5,c/b6}
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS \n
+ 			RET_INVALID_ARGUMENTS \n
+ 			RET_SHIFTING_FAILED */
+returnValue Constraints_shift(	Constraints* _THIS,
+								int offset		/**< Shift offset within the range [0,n/2] and integer divisor of n. */
+								);
+
+/** Rotates forward type and status of all constraints by a given
+ *  offset. This offset has to lie within the range [0,n].
+ *  Example for offset = 2: \n
+ *  rotate( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c1,c2}
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEX_OUT_OF_BOUNDS \n
+ 			RET_ROTATING_FAILED */
+returnValue Constraints_rotate(	Constraints* _THIS,
+								int offset		/**< Rotation offset within the range [0,n]. */
+								);
+
+
+/** Prints information on constraints object
+ *  (in particular, lists of inactive and active constraints.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INDEXLIST_CORRUPTED */
+returnValue Constraints_print(	Constraints* _THIS
+								);
+
+
+/** Initially adds all numbers of new (i.e. not yet in the list) bounds to
+ *  to the index set corresponding to the desired status;
+ *  the order depends on the SujectToType of each index.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SETUP_CONSTRAINT_FAILED */
+returnValue Constraints_setupAll(	Constraints* _THIS,
+									SubjectToStatus _status	/**< Desired initial status for all bounds. */
+									);
+
+
+/** Adds the index of a new constraint to index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Constraints_addIndex(	Constraints* _THIS,
+									Indexlist* const indexlist,	/**< Index list to which the new index shall be added. */
+									int newnumber,				/**< Number of new constraint. */
+									SubjectToStatus newstatus	/**< Status of new constraint. */
+									);
+
+/** Removes the index of a constraint from index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_REMOVEINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Constraints_removeIndex(	Constraints* _THIS,
+										Indexlist* const indexlist,	/**< Index list from which the new index shall be removed. */
+										int removenumber			/**< Number of constraint to be removed. */
+										);
+
+/** Swaps the indices of two constraints or bounds within the index set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_SWAPINDEX_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue Constraints_swapIndex(	Constraints* _THIS,
+									Indexlist* const indexlist,	/**< Index list in which the indices shold be swapped. */
+									int number1,				/**< Number of first constraint. */
+									int number2					/**< Number of second constraint. */
+									);
+
+
+
+/*
+ *	g e t N u m b e r O f T y p e
+ */
+static inline int Constraints_getNumberOfType( Constraints* _THIS, SubjectToType _type )
+{
+	int i;
+	int numberOfType = 0;
+
+	if ( _THIS->type != 0 )
+	{
+		for( i=0; i<_THIS->n; ++i )
+			if ( _THIS->type[i] == _type )
+				++numberOfType;
+	}
+
+	return numberOfType;
+}
+
+
+/*
+ *	g e t T y p e
+ */
+static inline SubjectToType Constraints_getType( Constraints* _THIS, int i )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+		return _THIS->type[i];
+
+	return ST_UNKNOWN;
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+static inline SubjectToStatus Constraints_getStatus( Constraints* _THIS, int i )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+		return _THIS->status[i];
+
+	return ST_UNDEFINED;
+}
+
+
+/*
+ *	s e t T y p e
+ */
+static inline returnValue Constraints_setType( Constraints* _THIS, int i, SubjectToType value )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+	{
+		_THIS->type[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t S t a t u s
+ */
+static inline returnValue Constraints_setStatus( Constraints* _THIS, int i, SubjectToStatus value )
+{
+	if ( ( i >= 0 ) && ( i < _THIS->n ) )
+	{
+		_THIS->status[i] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t N o L o w e r
+ */
+static inline void Constraints_setNoLower( Constraints* _THIS, BooleanType _status )
+{
+	_THIS->noLower = _status;
+}
+
+
+/*
+ *	s e t N o U p p e r
+ */
+static inline void Constraints_setNoUpper( Constraints* _THIS, BooleanType _status )
+{
+	_THIS->noUpper = _status;
+}
+
+
+/*
+ *	h a s N o L o w e r
+ */
+static inline BooleanType Constraints_hasNoLower( Constraints* _THIS )
+{
+	return _THIS->noLower;
+}
+
+
+/*
+ *	h a s N o U p p p e r
+ */
+static inline BooleanType Constraints_hasNoUpper( Constraints* _THIS )
+{
+	return _THIS->noUpper;
+}
+
+
+
+/*
+ *	g e t N C
+ */
+static inline int Constraints_getNC( Constraints* _THIS )
+{
+ 	return _THIS->n;
+}
+
+
+/*
+ *	g e t N E C
+ */
+static inline int Constraints_getNEC( Constraints* _THIS )
+{
+	return Constraints_getNumberOfType( _THIS,ST_EQUALITY );
+}
+
+
+/*
+ *	g e t N I C
+ */
+static inline int Constraints_getNIC( Constraints* _THIS )
+{
+ 	return Constraints_getNumberOfType( _THIS,ST_BOUNDED );
+}
+
+
+/*
+ *	g e t N U C
+ */
+static inline int Constraints_getNUC( Constraints* _THIS )
+{
+ 	return Constraints_getNumberOfType( _THIS,ST_UNBOUNDED );
+}
+
+
+/*
+ *	g e t N A C
+ */
+static inline int Constraints_getNAC( Constraints* _THIS )
+{
+ 	return Indexlist_getLength( _THIS->active );
+}
+
+
+/*
+ *	g e t N I A C
+ */
+static inline int Constraints_getNIAC( Constraints* _THIS )
+{
+	return Indexlist_getLength( _THIS->inactive );
+}
+
+
+
+/*
+ *	g e t A c t i v e
+ */
+static inline Indexlist* Constraints_getActive( Constraints* _THIS )
+{
+	return _THIS->active;
+}
+
+
+/*
+ *	g e t I n a c t i v e
+ */
+static inline Indexlist* Constraints_getInactive( Constraints* _THIS )
+{
+	return _THIS->inactive;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_CONSTRAINTS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Flipper.h b/third_party/acados/include/qpOASES_e/Flipper.h
new file mode 100644
index 000000000..63526a30f
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Flipper.h
@@ -0,0 +1,129 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Flipper.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Options class designed to manage user-specified
+ *	options for solving a QProblem.
+ */
+
+
+#ifndef QPOASES_FLIPPER_H
+#define QPOASES_FLIPPER_H
+
+
+#include <qpOASES_e/Bounds.h>
+#include <qpOASES_e/Constraints.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Auxiliary class for storing a copy of the current matrix factorisations.
+ *
+ *	This auxiliary class stores a copy of the current matrix factorisations. It
+ *	is used by the classe QProblemB and QProblem in case flipping bounds are enabled.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	Bounds      *bounds;			/**< Data structure for problem's bounds. */
+	Constraints *constraints;		/**< Data structure for problem's constraints. */
+
+	real_t *R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
+	real_t *Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
+	real_t *T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
+
+	unsigned int nV;				/**< Number of variables. */
+	unsigned int nC;				/**< Number of constraints. */
+} Flipper;
+
+int Flipper_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *Flipper_assignMemory( unsigned int nV, unsigned int nC, Flipper **mem, void *raw_memory );
+
+Flipper *Flipper_createMemory( unsigned int nV, unsigned int nC );
+
+/** Constructor which takes the number of bounds and constraints. */
+void FlipperCON(	Flipper* _THIS,
+					unsigned int _nV,		/**< Number of bounds. */
+					unsigned int _nC		/**< Number of constraints. */
+					);
+
+/** Copy constructor (deep copy). */
+void FlipperCPY(	Flipper* FROM,
+					Flipper* TO
+					);
+
+/** Initialises object with given number of bounds and constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INVALID_ARGUMENTS */
+returnValue Flipper_init(	Flipper* _THIS,
+							unsigned int _nV,	/**< Number of bounds. */
+							unsigned int _nC	/**< Number of constraints. */
+							);
+
+
+/** Copies current values to non-null arguments (assumed to be allocated with consistent size).
+ *	\return SUCCESSFUL_RETURN */
+returnValue Flipper_get(	Flipper* _THIS,
+							Bounds* const _bounds,				/**< Pointer to new bounds. */
+							real_t* const R,					/**< New matrix R. */
+							Constraints* const _constraints,	/**< Pointer to new constraints. */
+							real_t* const _Q,					/**< New matrix Q. */
+							real_t* const _T					/**< New matrix T. */
+							);
+
+/** Assigns new values to non-null arguments.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Flipper_set(	Flipper* _THIS,
+							const Bounds* const _bounds,			/**< Pointer to new bounds. */
+							const real_t* const _R,					/**< New matrix R. */
+							const Constraints* const _constraints,	/**< Pointer to new constraints. */
+							const real_t* const _Q,					/**< New matrix Q. */
+							const real_t* const _T					/**< New matrix T. */
+							);
+
+/** Returns dimension of matrix T.
+ *  \return Dimension of matrix T. */
+unsigned int Flipper_getDimT( Flipper* _THIS );
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_FLIPPER_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Indexlist.h b/third_party/acados/include/qpOASES_e/Indexlist.h
new file mode 100644
index 000000000..c3026a7ff
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Indexlist.h
@@ -0,0 +1,221 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Indexlist.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Indexlist class designed to manage index lists of
+ *	constraints and bounds within a SubjectTo object.
+ */
+
+
+#ifndef QPOASES_INDEXLIST_H
+#define QPOASES_INDEXLIST_H
+
+
+#include <qpOASES_e/Utils.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Stores and manages index lists.
+ *
+ *	This class manages index lists of active/inactive bounds/constraints.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	int *number;		/**< Array to store numbers of constraints or bounds. */
+	int *iSort;			/**< Index list to sort vector \a number */
+
+	int	length;			/**< Length of index list. */
+	int	first;			/**< Physical index of first element. */
+	int	last;			/**< Physical index of last element. */
+	int	lastusedindex;	/**< Physical index of last entry in index list. */
+	int	physicallength;	/**< Physical length of index list. */
+} Indexlist;
+
+int Indexlist_calculateMemorySize( int n);
+
+char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory);
+
+Indexlist *Indexlist_createMemory( int n );
+
+/** Constructor which takes the desired physical length of the index list. */
+void IndexlistCON(	Indexlist* _THIS,
+					int n	/**< Physical length of index list. */
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void IndexlistCPY(	Indexlist* FROM,
+					Indexlist* TO
+					);
+
+/** Initialises index list of desired physical length.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INVALID_ARGUMENTS */
+returnValue Indexlist_init(	Indexlist* _THIS,
+							int n		/**< Physical length of index list. */
+							);
+
+/** Creates an array of all numbers within the index set in correct order.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_CORRUPTED */
+returnValue Indexlist_getNumberArray(	Indexlist* _THIS,
+										int** const numberarray	/**< Output: Array of numbers (NULL on error). */
+										);
+
+/** Creates an array of all numbers within the index set in correct order.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INDEXLIST_CORRUPTED */
+returnValue	Indexlist_getISortArray(	Indexlist* _THIS,
+										int** const iSortArray	/**< Output: iSort Array. */
+										);
+
+
+/** Determines the index within the index list at which a given number is stored.
+ *	\return >= 0: Index of given number. \n
+ 			-1: Number not found. */
+int Indexlist_getIndex(	Indexlist* _THIS,
+						int givennumber	/**< Number whose index shall be determined. */
+						);
+
+/** Returns the number stored at a given physical index.
+ *	\return >= 0: Number stored at given physical index. \n
+ 			-RET_INDEXLIST_OUTOFBOUNDS */
+static inline int Indexlist_getNumber(	Indexlist* _THIS,
+										int physicalindex	/**< Physical index of the number to be returned. */
+										);
+
+
+/** Returns the current length of the index list.
+ *	\return Current length of the index list. */
+static inline int Indexlist_getLength(	Indexlist* _THIS
+										);
+
+/** Returns last number within the index list.
+ *	\return Last number within the index list. */
+static inline int Indexlist_getLastNumber(	Indexlist* _THIS
+											);
+
+
+/** Adds number to index list.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_MUST_BE_REORDERD \n
+ 			RET_INDEXLIST_EXCEEDS_MAX_LENGTH */
+returnValue Indexlist_addNumber(	Indexlist* _THIS,
+									int addnumber			/**< Number to be added. */
+									);
+
+/** Removes number from index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Indexlist_removeNumber(	Indexlist* _THIS,
+									int removenumber	/**< Number to be removed. */
+									);
+
+/** Swaps two numbers within index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Indexlist_swapNumbers(	Indexlist* _THIS,
+									int number1,		/**< First number for swapping. */
+									int number2			/**< Second number for swapping. */
+									);
+
+/** Determines if a given number is contained in the index set.
+ *	\return BT_TRUE iff number is contain in the index set */
+static inline BooleanType Indexlist_isMember(	Indexlist* _THIS,
+												int _number		/**< Number to be tested for membership. */
+												);
+
+
+/** Find first index j between -1 and length in sorted list of indices
+ *  iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses
+ *  bisection.
+ *  \return j. */
+int Indexlist_findInsert(	Indexlist* _THIS,
+							int i
+							);
+
+
+
+/*
+ *	g e t N u m b e r
+ */
+static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex )
+{
+	/* consistency check */
+	if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) )
+		return -RET_INDEXLIST_OUTOFBOUNDS;
+
+	return _THIS->number[physicalindex];
+}
+
+
+/*
+ *	g e t L e n g t h
+ */
+static inline int Indexlist_getLength( Indexlist* _THIS )
+{
+	return _THIS->length;
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+static inline int Indexlist_getLastNumber( Indexlist* _THIS )
+{
+	return _THIS->number[_THIS->length-1];
+}
+
+
+/*
+ *	g e t L a s t N u m b e r
+ */
+static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number )
+{
+	if ( Indexlist_getIndex( _THIS,_number ) >= 0 )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_INDEXLIST_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Matrices.h b/third_party/acados/include/qpOASES_e/Matrices.h
new file mode 100644
index 000000000..1d6da8c3c
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Matrices.h
@@ -0,0 +1,287 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Matrices.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2009-2015
+ *
+ *  Various matrix classes: Abstract base matrix class, dense and sparse matrices,
+ *  including symmetry exploiting specializations.
+ */
+
+
+
+#ifndef QPOASES_MATRICES_H
+#define QPOASES_MATRICES_H
+
+#ifdef __USE_SINGLE_PRECISION__
+
+	// single precision
+	#define GEMM sgemm_
+	#define GEMV sgemv_
+//	#define SYR ssyr_
+//	#define SYR2 ssyr2_
+	#define POTRF spotrf_
+
+#else
+
+	// double precision
+	#define GEMM dgemm_
+	#define GEMV dgemv_
+//	#define SYR  dsyr_
+//	#define SYR2 dsyr2_
+	#define POTRF dpotrf_
+
+#endif /* __USE_SINGLE_PRECISION__ */
+
+
+#ifdef EXTERNAL_BLAS
+	// double precision
+	void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc);
+	void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy);
+	void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info);
+	// single precision
+	void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc);
+	void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy);
+	void spotrf_(char *uplo, int *m, float *A, int *lda, int *info);
+#else
+	/** Performs one of the matrix-matrix operation in double precision. */
+	void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const double*, const double*, const unsigned long*, const double*, const unsigned long*,
+			const double*, double*, const unsigned long* );
+	/** Performs one of the matrix-matrix operation in single precision. */
+	void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*,
+			const float*, const float*, const unsigned long*, const float*, const unsigned long*,
+			const float*, float*, const unsigned long* );
+
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */
+	void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * );
+	/** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */
+	void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * );
+
+#endif
+
+	/** Performs a symmetric rank 1 operation in double precision. */
+//	void dsyr_ ( const char *, const unsigned long *, const double *, const double *,
+//				 const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 1 operation in single precision. */
+//	void ssyr_ ( const char *, const unsigned long *, const float *, const float *,
+//				 const unsigned long *, float *, const unsigned long *);
+
+	/** Performs a symmetric rank 2 operation in double precision. */
+//	void dsyr2_ ( const char *, const unsigned long *, const double *, const double *,
+//				  const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *);
+	/** Performs a symmetric rank 2 operation in single precision. */
+//	void ssyr2_ ( const char *, const unsigned long *, const float *, const float *,
+//				  const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *);
+
+
+#include <qpOASES_e/Indexlist.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Interfaces matrix-vector operations tailored to general dense matrices.
+ *
+ *	Dense matrix class (row major format).
+ *
+ *	\author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2011-2015
+ */
+typedef struct
+{
+	real_t *val;				/**< Vector of entries. */
+	int nRows;					/**< Number of rows. */
+	int nCols;					/**< Number of columns. */
+	int leaDim;					/**< Leading dimension. */
+} DenseMatrix;
+
+int DenseMatrix_calculateMemorySize( int m, int n );
+
+char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory );
+
+DenseMatrix *DenseMatrix_createMemory( int m, int n );
+
+/** Constructor from vector of values.
+ *  Caution: Data pointer must be valid throughout lifetime
+ */
+void DenseMatrixCON(	DenseMatrix* _THIS,
+						int m,			/**< Number of rows. */
+						int n,			/**< Number of columns. */
+						int lD,			/**< Leading dimension. */
+						real_t *v		/**< Values. */
+						);
+
+void DenseMatrixCPY(	DenseMatrix* FROM,
+						DenseMatrix* TO
+						);
+
+
+/** Frees all internal memory. */
+void DenseMatrix_free( DenseMatrix* _THIS );
+
+/** Constructor from vector of values.
+ *  Caution: Data pointer must be valid throughout lifetime
+ */
+returnValue DenseMatrix_init(	DenseMatrix* _THIS,
+								int m,			/**< Number of rows. */
+								int n,			/**< Number of columns. */
+								int lD,			/**< Leading dimension. */
+								real_t *v		/**< Values. */
+								);
+
+
+/** Returns i-th diagonal entry.
+ *	\return i-th diagonal entry */
+real_t DenseMatrix_diag(	DenseMatrix* _THIS,
+							int i			/**< Index. */
+							);
+
+/** Checks whether matrix is square and diagonal.
+ *	\return BT_TRUE  iff matrix is square and diagonal; \n
+ *	        BT_FALSE otherwise. */
+BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS );
+
+/** Get the N-norm of the matrix
+ *  \return N-norm of the matrix
+ */
+real_t DenseMatrix_getNorm( DenseMatrix* _THIS,
+							int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+							);
+
+/** Get the N-norm of a row
+ *  \return N-norm of row \a rNum
+ */
+real_t DenseMatrix_getRowNorm(	DenseMatrix* _THIS,
+								int rNum,			/**< Row number. */
+								int type			/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+								);
+
+/** Retrieve indexed entries of matrix row multiplied by alpha.
+ *  \return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_getRow(	DenseMatrix* _THIS,
+								int rNum,						/**< Row number. */
+								const Indexlist* const icols,	/**< Index list specifying columns. */
+								real_t alpha,					/**< Scalar factor. */
+								real_t *row						/**< Output row vector. */
+								);
+
+/** Retrieve indexed entries of matrix column multiplied by alpha.
+ *  \return SUCCESSFUL_RETURN */
+ returnValue DenseMatrix_getCol(	DenseMatrix* _THIS,
+									int cNum,						/**< Column number. */
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									real_t alpha,					/**< Scalar factor. */
+									real_t *col						/**< Output column vector. */
+									);
+
+/** Evaluate Y=alpha*A*X + beta*Y.
+ *  \return SUCCESSFUL_RETURN. */
+returnValue DenseMatrix_times(	DenseMatrix* _THIS,
+								int xN,					/**< Number of vectors to multiply. */
+								real_t alpha,			/**< Scalar factor for matrix vector product. */
+								const real_t *x,		/**< Input vector to be multiplied. */
+								int xLD,				/**< Leading dimension of input x. */
+								real_t beta,			/**< Scalar factor for y. */
+								real_t *y,				/**< Output vector of results. */
+								int yLD					/**< Leading dimension of output y. */
+								);
+
+/** Evaluate Y=alpha*A'*X + beta*Y.
+ *  \return SUCCESSFUL_RETURN. */
+returnValue DenseMatrix_transTimes(	DenseMatrix* _THIS,
+									int xN,				/**< Number of vectors to multiply. */
+									real_t alpha,		/**< Scalar factor for matrix vector product. */
+									const real_t *x,	/**< Input vector to be multiplied. */
+									int xLD,			/**< Leading dimension of input x. */
+									real_t beta,		/**< Scalar factor for y. */
+									real_t *y,			/**< Output vector of results. */
+									int yLD				/**< Leading dimension of output y. */
+									);
+
+/** Evaluate matrix vector product with submatrix given by Indexlist.
+ *	\return SUCCESSFUL_RETURN */
+ returnValue DenseMatrix_subTimes(	DenseMatrix* _THIS,
+									const Indexlist* const irows,	/**< Index list specifying rows. */
+									const Indexlist* const icols,	/**< Index list specifying columns. */
+									int xN,							/**< Number of vectors to multiply. */
+									real_t alpha,					/**< Scalar factor for matrix vector product. */
+									const real_t *x,				/**< Input vector to be multiplied. */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t beta,					/**< Scalar factor for y. */
+									real_t *y,						/**< Output vector of results. */
+									int yLD,						/**< Leading dimension of output y. */
+									BooleanType yCompr 				/**< Compressed storage for y. */
+									);
+
+/** Evaluate matrix transpose vector product.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_subTransTimes(	DenseMatrix* _THIS,
+										const Indexlist* const irows,	/**< Index list specifying rows. */
+										const Indexlist* const icols,	/**< Index list specifying columns. */
+										int xN,							/**< Number of vectors to multiply. */
+										real_t alpha,					/**< Scalar factor for matrix vector product. */
+										const real_t *x,				/**< Input vector to be multiplied. */
+										int xLD,						/**< Leading dimension of input x. */
+										real_t beta,					/**< Scalar factor for y. */
+										real_t *y,						/**< Output vector of results. */
+										int yLD							/**< Leading dimension of output y. */
+										);
+
+/** Adds given offset to diagonal of matrix.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_NO_DIAGONAL_AVAILABLE */
+returnValue DenseMatrix_addToDiag(	DenseMatrix* _THIS,
+									real_t alpha		/**< Diagonal offset. */
+									);
+
+/** Prints matrix to screen.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_print(	DenseMatrix* _THIS
+								);
+
+static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; }
+
+/** Compute bilinear form y = x'*H*x using submatrix given by index list.
+ *	\return SUCCESSFUL_RETURN */
+returnValue DenseMatrix_bilinear(	DenseMatrix* _THIS,
+									const Indexlist* const icols,	/**< Index list specifying columns of x. */
+									int xN,							/**< Number of vectors to multiply. */
+									const real_t *x,				/**< Input vector to be multiplied (uncompressed). */
+									int xLD,						/**< Leading dimension of input x. */
+									real_t *y,						/**< Output vector of results (compressed). */
+									int yLD							/**< Leading dimension of output y. */
+									);
+
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_MATRICES_H */
diff --git a/third_party/acados/include/qpOASES_e/MessageHandling.h b/third_party/acados/include/qpOASES_e/MessageHandling.h
new file mode 100644
index 000000000..fe5524948
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/MessageHandling.h
@@ -0,0 +1,544 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/MessageHandling.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Leonard Wirsching)
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the MessageHandling class including global return values.
+ */
+
+
+#ifndef QPOASES_MESSAGEHANDLING_H
+#define QPOASES_MESSAGEHANDLING_H
+
+
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+
+#include <qpOASES_e/Constants.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Default file to display messages. */
+#define stdFile stderr
+
+
+/**
+ *	\brief Defines all symbols for global return values.
+ *	
+ *  The enumeration returnValueType defines all symbols for global return values.
+ *	Important: All return values are assumed to be nonnegative!
+ *
+ *	\author Hans Joachim Ferreau
+ */
+typedef enum 
+{
+TERMINAL_LIST_ELEMENT = -1,						/**< Terminal list element, internal usage only! */
+/* miscellaneous */
+SUCCESSFUL_RETURN = 0,							/**< Successful return. */
+RET_DIV_BY_ZERO,		   						/**< Division by zero. */
+RET_INDEX_OUT_OF_BOUNDS,						/**< Index out of bounds. */
+RET_INVALID_ARGUMENTS,							/**< At least one of the arguments is invalid. */
+RET_ERROR_UNDEFINED,							/**< Error number undefined. */
+RET_WARNING_UNDEFINED,							/**< Warning number undefined. */
+RET_INFO_UNDEFINED,								/**< Info number undefined. */
+RET_EWI_UNDEFINED,								/**< Error/warning/info number undefined. */
+RET_AVAILABLE_WITH_LINUX_ONLY,					/**< This function is available under Linux only. */
+RET_UNKNOWN_BUG,								/**< The error occurred is not yet known. */
+RET_PRINTLEVEL_CHANGED,							/**< Print level changed. (10) */
+RET_NOT_YET_IMPLEMENTED,						/**< Requested function is not yet implemented in this version of qpOASES. */
+/* Indexlist */
+RET_INDEXLIST_MUST_BE_REORDERD,					/**< Index list has to be reordered. */
+RET_INDEXLIST_EXCEEDS_MAX_LENGTH,				/**< Index list exceeds its maximal physical length. */
+RET_INDEXLIST_CORRUPTED,						/**< Index list corrupted. */
+RET_INDEXLIST_OUTOFBOUNDS,						/**< Physical index is out of bounds. */
+RET_INDEXLIST_ADD_FAILED,						/**< Adding indices from another index set failed. */
+RET_INDEXLIST_INTERSECT_FAILED,					/**< Intersection with another index set failed. */
+/* SubjectTo / Bounds / Constraints */
+RET_INDEX_ALREADY_OF_DESIRED_STATUS,			/**< Index is already of desired status. (18) */
+RET_ADDINDEX_FAILED,							/**< Adding index to index set failed. */
+RET_REMOVEINDEX_FAILED,							/**< Removing index from index set failed. (20) */
+RET_SWAPINDEX_FAILED,							/**< Cannot swap between different indexsets. */
+RET_NOTHING_TO_DO,								/**< Nothing to do. */
+RET_SETUP_BOUND_FAILED,							/**< Setting up bound index failed. */
+RET_SETUP_CONSTRAINT_FAILED,					/**< Setting up constraint index failed. */
+RET_MOVING_BOUND_FAILED,						/**< Moving bound between index sets failed. */
+RET_MOVING_CONSTRAINT_FAILED,					/**< Moving constraint between index sets failed. */
+RET_SHIFTING_FAILED,							/**< Shifting of bounds/constraints failed. */
+RET_ROTATING_FAILED,							/**< Rotating of bounds/constraints failed. */
+/* QProblem */
+RET_QPOBJECT_NOT_SETUP,							/**< The QP object has not been setup correctly, use another constructor. */
+RET_QP_ALREADY_INITIALISED,						/**< QProblem has already been initialised. (30) */
+RET_NO_INIT_WITH_STANDARD_SOLVER,				/**< Initialisation via extern QP solver is not yet implemented. */
+RET_RESET_FAILED,								/**< Reset failed. */
+RET_INIT_FAILED,								/**< Initialisation failed. */
+RET_INIT_FAILED_TQ,								/**< Initialisation failed due to TQ factorisation. */
+RET_INIT_FAILED_CHOLESKY,						/**< Initialisation failed due to Cholesky decomposition. */
+RET_INIT_FAILED_HOTSTART,						/**< Initialisation failed! QP could not be solved! */
+RET_INIT_FAILED_INFEASIBILITY,					/**< Initial QP could not be solved due to infeasibility! */
+RET_INIT_FAILED_UNBOUNDEDNESS,					/**< Initial QP could not be solved due to unboundedness! */
+RET_INIT_FAILED_REGULARISATION,					/**< Initialisation failed as Hessian matrix could not be regularised. */
+RET_INIT_SUCCESSFUL,							/**< Initialisation done. (40) */
+RET_OBTAINING_WORKINGSET_FAILED,				/**< Failed to obtain working set for auxiliary QP. */
+RET_SETUP_WORKINGSET_FAILED,					/**< Failed to setup working set for auxiliary QP. */
+RET_SETUP_AUXILIARYQP_FAILED,					/**< Failed to setup auxiliary QP for initialised homotopy. */
+RET_NO_CHOLESKY_WITH_INITIAL_GUESS,				/**< Externally computed Cholesky factor cannot be combined with an initial guess. */
+RET_NO_EXTERN_SOLVER,							/**< No extern QP solver available. */
+RET_QP_UNBOUNDED,								/**< QP is unbounded. */
+RET_QP_INFEASIBLE,								/**< QP is infeasible. */
+RET_QP_NOT_SOLVED,								/**< Problems occurred while solving QP with standard solver. */
+RET_QP_SOLVED,									/**< QP successfully solved. */
+RET_UNABLE_TO_SOLVE_QP,							/**< Problems occurred while solving QP. (50) */
+RET_INITIALISATION_STARTED,						/**< Starting problem initialisation... */
+RET_HOTSTART_FAILED,							/**< Unable to perform homotopy due to internal error. */
+RET_HOTSTART_FAILED_TO_INIT,					/**< Unable to initialise problem. */
+RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED,		/**< Unable to perform homotopy as previous QP is not solved. */
+RET_ITERATION_STARTED,							/**< Iteration... */
+RET_SHIFT_DETERMINATION_FAILED,					/**< Determination of shift of the QP data failed. */
+RET_STEPDIRECTION_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
+RET_STEPLENGTH_DETERMINATION_FAILED,			/**< Determination of step direction failed. */
+RET_OPTIMAL_SOLUTION_FOUND,						/**< Optimal solution of neighbouring QP found. */
+RET_HOMOTOPY_STEP_FAILED,						/**< Unable to perform homotopy step. (60) */
+RET_HOTSTART_STOPPED_INFEASIBILITY,				/**< Premature homotopy termination because QP is infeasible. */
+RET_HOTSTART_STOPPED_UNBOUNDEDNESS,				/**< Premature homotopy termination because QP is unbounded. */
+RET_WORKINGSET_UPDATE_FAILED,					/**< Unable to update working sets according to initial guesses. */
+RET_MAX_NWSR_REACHED,							/**< Maximum number of working set recalculations performed. */
+RET_CONSTRAINTS_NOT_SPECIFIED,					/**< Problem does comprise constraints! You also have to specify new constraints' bounds. */
+RET_INVALID_FACTORISATION_FLAG,					/**< Invalid factorisation flag. */
+RET_UNABLE_TO_SAVE_QPDATA,						/**< Unable to save QP data. */
+RET_STEPDIRECTION_FAILED_TQ,					/**< Abnormal termination due to TQ factorisation. */
+RET_STEPDIRECTION_FAILED_CHOLESKY,				/**< Abnormal termination due to Cholesky factorisation. */
+RET_CYCLING_DETECTED,							/**< Cycling detected. (70) */
+RET_CYCLING_NOT_RESOLVED,						/**< Cycling cannot be resolved, QP probably infeasible. */
+RET_CYCLING_RESOLVED,							/**< Cycling probably resolved. */
+RET_STEPSIZE,									/**< For displaying performed stepsize. */
+RET_STEPSIZE_NONPOSITIVE,						/**< For displaying non-positive stepsize. */
+RET_SETUPSUBJECTTOTYPE_FAILED,					/**< Setup of SubjectToTypes failed. */
+RET_ADDCONSTRAINT_FAILED,						/**< Addition of constraint to working set failed. */
+RET_ADDCONSTRAINT_FAILED_INFEASIBILITY,			/**< Addition of constraint to working set failed (due to QP infeasibility). */
+RET_ADDBOUND_FAILED,							/**< Addition of bound to working set failed. */
+RET_ADDBOUND_FAILED_INFEASIBILITY,				/**< Addition of bound to working set failed (due to QP infeasibility). */
+RET_REMOVECONSTRAINT_FAILED,					/**< Removal of constraint from working set failed. (80) */
+RET_REMOVEBOUND_FAILED,							/**< Removal of bound from working set failed. */
+RET_REMOVE_FROM_ACTIVESET,						/**< Removing from active set... */
+RET_ADD_TO_ACTIVESET,							/**< Adding to active set... */
+RET_REMOVE_FROM_ACTIVESET_FAILED,				/**< Removing from active set failed. */
+RET_ADD_TO_ACTIVESET_FAILED,					/**< Adding to active set failed. */
+RET_CONSTRAINT_ALREADY_ACTIVE,					/**< Constraint is already active. */
+RET_ALL_CONSTRAINTS_ACTIVE,						/**< All constraints are active, no further constraint can be added. */
+RET_LINEARLY_DEPENDENT,							/**< New bound/constraint is linearly dependent. */
+RET_LINEARLY_INDEPENDENT,						/**< New bound/constraint is linearly independent. */
+RET_LI_RESOLVED,								/**< Linear independence of active constraint matrix successfully resolved. (90) */
+RET_ENSURELI_FAILED,							/**< Failed to ensure linear independence of active constraint matrix. */
+RET_ENSURELI_FAILED_TQ,							/**< Abnormal termination due to TQ factorisation. */
+RET_ENSURELI_FAILED_NOINDEX,					/**< QP is infeasible. */
+RET_ENSURELI_FAILED_CYCLING,					/**< QP is infeasible. */
+RET_BOUND_ALREADY_ACTIVE,						/**< Bound is already active. */
+RET_ALL_BOUNDS_ACTIVE,							/**< All bounds are active, no further bound can be added. */
+RET_CONSTRAINT_NOT_ACTIVE,						/**< Constraint is not active. */
+RET_BOUND_NOT_ACTIVE,							/**< Bound is not active. */
+RET_HESSIAN_NOT_SPD,							/**< Projected Hessian matrix not positive definite. */
+RET_HESSIAN_INDEFINITE,							/**< Hessian matrix is indefinite. (100) */
+RET_MATRIX_SHIFT_FAILED,						/**< Unable to update matrices or to transform vectors. */
+RET_MATRIX_FACTORISATION_FAILED,				/**< Unable to calculate new matrix factorisations. */
+RET_PRINT_ITERATION_FAILED,						/**< Unable to print information on current iteration. */
+RET_NO_GLOBAL_MESSAGE_OUTPUTFILE,				/**< No global message output file initialised. */
+RET_DISABLECONSTRAINTS_FAILED,					/**< Unable to disbable constraints. */
+RET_ENABLECONSTRAINTS_FAILED,					/**< Unable to enbable constraints. */
+RET_ALREADY_ENABLED,							/**< Bound or constraint is already enabled. */
+RET_ALREADY_DISABLED,							/**< Bound or constraint is already disabled. */
+RET_NO_HESSIAN_SPECIFIED, 						/**< No Hessian matrix has been specified. */
+RET_USING_REGULARISATION,						/**< Using regularisation as Hessian matrix is not positive definite. (110) */
+RET_EPS_MUST_BE_POSITVE,						/**< Eps for regularisation must be sufficiently positive. */
+RET_REGSTEPS_MUST_BE_POSITVE, 					/**< Maximum number of regularisation steps must be non-negative. */
+RET_HESSIAN_ALREADY_REGULARISED,				/**< Hessian has been already regularised. */
+RET_CANNOT_REGULARISE_IDENTITY,					/**< Identity Hessian matrix cannot be regularised. */
+RET_CANNOT_REGULARISE_SPARSE,					/**< Sparse matrix cannot be regularised as diagonal entry is missing. */
+RET_NO_REGSTEP_NWSR,							/**< No additional regularisation step could be performed due to limits. */
+RET_FEWER_REGSTEPS_NWSR,						/**< Fewer additional regularisation steps have been performed due to limits. */
+RET_CHOLESKY_OF_ZERO_HESSIAN, 					/**< Cholesky decomposition of (unregularised) zero Hessian matrix. */
+RET_ZERO_HESSIAN_ASSUMED,						/**< Zero Hessian matrix assumed as null pointer passed without specifying hessianType. */
+RET_CONSTRAINTS_ARE_NOT_SCALED, 				/**< (no longer in use) (120) */
+RET_INITIAL_BOUNDS_STATUS_NYI, 					/**< (no longer in use) */
+RET_ERROR_IN_CONSTRAINTPRODUCT,					/**< Error in user-defined constraint product function. */
+RET_FIX_BOUNDS_FOR_LP,							/**< All initial bounds must be fixed when solving an (unregularised) LP. */
+RET_USE_REGULARISATION_FOR_LP,					/**< Set options.enableRegularisation=BT_TRUE for solving LPs. */
+/* SQProblem */
+RET_UPDATEMATRICES_FAILED,						/**< Unable to update QP matrices. */
+RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED,		/**< Unable to update matrices as previous QP is not solved. */
+/* Utils */
+RET_UNABLE_TO_OPEN_FILE,						/**< Unable to open file. */
+RET_UNABLE_TO_WRITE_FILE,						/**< Unable to write into file. */
+RET_UNABLE_TO_READ_FILE,						/**< Unable to read from file. */
+RET_FILEDATA_INCONSISTENT,						/**< File contains inconsistent data. (130) */
+/* Options */
+RET_OPTIONS_ADJUSTED,							/**< Options needed to be adjusted for consistency reasons. */
+/* SolutionAnalysis */
+RET_UNABLE_TO_ANALYSE_QPROBLEM, 				/**< Unable to analyse (S)QProblem(B) object. */
+/* Benchmark */
+RET_NWSR_SET_TO_ONE,							/**< Maximum number of working set changes was set to 1. */
+RET_UNABLE_TO_READ_BENCHMARK,					/**< Unable to read benchmark data. */
+RET_BENCHMARK_ABORTED,							/**< Benchmark aborted. */
+RET_INITIAL_QP_SOLVED,							/**< Initial QP solved. */
+RET_QP_SOLUTION_STARTED,						/**< Solving QP... */
+RET_BENCHMARK_SUCCESSFUL,						/**< Benchmark terminated successfully. */
+/* Sparse matrices */
+RET_NO_DIAGONAL_AVAILABLE,						/**< Sparse matrix does not have entries on full diagonal. */
+RET_DIAGONAL_NOT_INITIALISED,					/**< Diagonal data of sparse matrix has not been initialised. (140) */
+/* Dropping of infeasible constraints */
+RET_ENSURELI_DROPPED,							/**< Linear independence resolved by dropping blocking constraint. */
+/* Simple exitflags */
+RET_SIMPLE_STATUS_P1,							/**< QP problem could not be solved within given number of iterations. */
+RET_SIMPLE_STATUS_P0,							/**< QP problem solved. */
+RET_SIMPLE_STATUS_M1,							/**< QP problem could not be solved due to an internal error. */
+RET_SIMPLE_STATUS_M2,							/**< QP problem is infeasible (and thus could not be solved). */
+RET_SIMPLE_STATUS_M3 							/**< QP problem is unbounded (and thus could not be solved). (146) */
+} returnValue;
+
+
+/**
+ *	\brief Data structure for entries in global message list.
+ *	
+ *	Data structure for entries in global message list.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	returnValue key;							/**< Global return value. */
+	const char* data;							/**< Corresponding message. */
+	VisibilityStatus globalVisibilityStatus; 	/**< Determines if message can be printed.
+												* 	 If this value is set to VS_HIDDEN, no message is printed! */
+} ReturnValueList;
+
+
+
+/** 
+ *	\brief Handles all kind of error messages, warnings and other information.
+ *
+ *	This class handles all kinds of messages (errors, warnings, infos) initiated
+ *  by qpOASES modules and stores the corresponding global preferences.
+ *
+ *	\author Hans Joachim Ferreau (thanks to Leonard Wirsching)
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	VisibilityStatus errorVisibility;		/**< Error messages visible? */
+	VisibilityStatus warningVisibility;		/**< Warning messages visible? */
+	VisibilityStatus infoVisibility;		/**< Info messages visible? */
+
+	FILE* outputFile;						/**< Output file for messages. */
+
+	int errorCount; 						/**< Counts number of errors (for nicer output only). */
+} MessageHandling;
+
+
+
+/** Constructor which takes the desired output file and desired visibility states. */
+void MessageHandlingCON(	MessageHandling* _THIS,
+							FILE* _outputFile,					/**< Output file. */
+							VisibilityStatus _errorVisibility,	/**< Visibility status for error messages. */
+							VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */
+							VisibilityStatus _infoVisibility	/**< Visibility status for info messages. */
+							);
+
+void MessageHandlingCPY(	MessageHandling* FROM,
+							MessageHandling* TO
+							);
+
+
+/** Prints an error message(a simplified macro THROWERROR is also provided). \n
+ *  Errors are definied as abnormal events which cause an immediate termination of the current (sub) function.
+ *  Errors of a sub function should be commented by the calling function by means of a warning message
+ *  (if this error does not cause an error of the calling function, either)!
+ *  \return Error number returned by sub function call
+ */
+returnValue MessageHandling_throwError(	MessageHandling* _THIS,
+										returnValue Enumber,					/**< Error number returned by sub function call. */
+										const char* additionaltext,				/**< Additional error text (0, if none). */
+										const char* functionname,				/**< Name of function which caused the error. */
+										const char* filename,					/**< Name of file which caused the error. */
+										const unsigned long linenumber,			/**< Number of line which caused the error.incompatible binary file */
+										VisibilityStatus localVisibilityStatus	/**< Determines (locally) if error message can be printed to stderr.
+																			*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																			*   no message is printed, anyway! */
+										);
+
+/** Prints a warning message (a simplified macro THROWWARNING is also provided).
+ *  Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function.
+ *  \return Warning number returned by sub function call
+ */
+returnValue MessageHandling_throwWarning(	MessageHandling* _THIS,
+											returnValue Wnumber,	 				/**< Warning number returned by sub function call. */
+											const char* additionaltext,				/**< Additional warning text (0, if none). */
+											const char* functionname,				/**< Name of function which caused the warning. */
+											const char* filename,   				/**< Name of file which caused the warning. */
+											const unsigned long linenumber,	 		/**< Number of line which caused the warning. */
+											VisibilityStatus localVisibilityStatus	/**< Determines (locally) if warning message can be printed to stderr.
+																					*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																					*   no message is printed, anyway! */
+											);
+
+/** Prints a info message (a simplified macro THROWINFO is also provided).
+ *  \return Info number returned by sub function call
+ */
+returnValue MessageHandling_throwInfo(	MessageHandling* _THIS,
+										returnValue Inumber,	 				/**< Info number returned by sub function call. */
+										const char* additionaltext,	 			/**< Additional warning text (0, if none). */
+										const char* functionname,				/**< Name of function which submitted the info. */
+										const char* filename,   				/**< Name of file which submitted the info. */
+										const unsigned long linenumber,			/**< Number of line which submitted the info. */
+										VisibilityStatus localVisibilityStatus	/**< Determines (locally) if info message can be printed to stderr.
+																				*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																				*   no message is printed, anyway! */
+										);
+
+
+/** Resets all preferences to default values.
+ *	\return SUCCESSFUL_RETURN */
+returnValue MessageHandling_reset( MessageHandling* _THIS );
+
+
+/** Prints a complete list of all messages to output file.
+ *	\return SUCCESSFUL_RETURN */
+returnValue MessageHandling_listAllMessages( MessageHandling* _THIS );
+
+
+/** Returns visibility status for error messages.
+ *	\return Visibility status for error messages. */
+static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS );
+
+/** Returns visibility status for warning messages.
+ *	\return Visibility status for warning messages. */
+static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS );
+
+/** Returns visibility status for info messages.
+ *	\return Visibility status for info messages. */
+static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS );
+
+/** Returns pointer to output file.
+ *	\return Pointer to output file. */
+static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS );
+
+/** Returns error count value.
+ *	\return Error count value. */
+static inline int MessageHandling_getErrorCount( MessageHandling* _THIS );
+
+
+/** Changes visibility status for error messages. */
+static inline void MessageHandling_setErrorVisibilityStatus(	MessageHandling* _THIS,
+														VisibilityStatus _errorVisibility	/**< New visibility status for error messages. */
+														);
+
+/** Changes visibility status for warning messages. */
+static inline void MessageHandling_setWarningVisibilityStatus(	MessageHandling* _THIS,
+																VisibilityStatus _warningVisibility	/**< New visibility status for warning messages. */
+																);
+
+/** Changes visibility status for info messages. */
+static inline void MessageHandling_setInfoVisibilityStatus(	MessageHandling* _THIS,
+															VisibilityStatus _infoVisibility	/**< New visibility status for info messages. */
+															);
+
+/** Changes output file for messages. */
+static inline void MessageHandling_setOutputFile(	MessageHandling* _THIS,
+													FILE* _outputFile	/**< New output file for messages. */
+													);
+
+/** Changes error count.
+ * \return SUCCESSFUL_RETURN \n
+ *		   RET_INVALID_ARGUMENT */
+static inline returnValue MessageHandling_setErrorCount(	MessageHandling* _THIS,
+															int _errorCount	/**< New error count value. */
+															);
+
+/** Provides message text corresponding to given \a returnValue.
+ * \return String containing message text. */
+const char* MessageHandling_getErrorCodeMessage(	MessageHandling* _THIS,
+													const returnValue _returnValue
+													);
+
+
+returnValue MessageHandling_throwMessage(	MessageHandling* _THIS,
+											returnValue RETnumber,	 				/**< Error/warning/info number returned by sub function call. */
+											const char* additionaltext,				/**< Additional warning text (0, if none). */
+											const char* functionname,				/**< Name of function which caused the error/warning/info. */
+											const char* filename,   				/**< Name of file which caused the error/warning/info. */
+											const unsigned long linenumber,			/**< Number of line which caused the error/warning/info. */
+											VisibilityStatus localVisibilityStatus,	/**< Determines (locally) if info message can be printed to stderr.
+																					*   If GLOBAL visibility status of the message is set to VS_HIDDEN,
+																					*   no message is printed, anyway! */
+											const char* RETstring					/**< Leading string of error/warning/info message. */
+											);
+
+
+#ifndef __FILE__
+  /** Ensures that __FILE__ macro is defined. */
+  #define __FILE__ 0
+#endif
+
+#ifndef __LINE__
+  /** Ensures that __LINE__ macro is defined. */
+  #define __LINE__ 0
+#endif
+
+/** Define __FUNC__ macro providing current function for debugging. */
+/*#define __FUNC__ 0*/
+#define __FUNC__ ("(no function name provided)")
+/*#define __FUNC__ __func__*/
+/*#define __FUNC__ __FUNCTION__*/
+
+
+/** Short version of throwError with default values, only returnValue is needed */
+#define THROWERROR(retval) ( MessageHandling_throwError( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) )
+
+/** Short version of throwWarning with default values, only returnValue is needed */
+#define THROWWARNING(retval) ( MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) )
+
+/** Short version of throwInfo with default values, only returnValue is needed */
+#define THROWINFO(retval) ( MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) )
+
+
+/** Returns a pointer to global message handler.
+ *  \return Pointer to global message handler.
+ */
+MessageHandling* qpOASES_getGlobalMessageHandler( );
+
+
+/*
+ *	g e t E r r o r V i s i b i l i t y S t a t u s
+ */
+static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS )
+{
+ 	return _THIS->errorVisibility;
+}
+
+
+/*
+ *	g e t W a r n i n g V i s i b i l i t y S t a t u s
+ */
+static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS )
+{
+ 	return _THIS->warningVisibility;
+}
+
+
+/*
+ *	g e t I n f o V i s i b i l i t y S t a t u s
+ */
+static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS )
+{
+ 	return _THIS->infoVisibility;
+}
+
+
+/*
+ *	g e t O u t p u t F i l e
+ */
+static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS )
+{
+ 	return _THIS->outputFile;
+}
+
+
+/*
+ *	g e t E r r o r C o u n t
+ */
+static inline int MessageHandling_getErrorCount( MessageHandling* _THIS )
+{
+ 	return _THIS->errorCount;
+}
+
+
+/*
+ *	s e t E r r o r V i s i b i l i t y S t a t u s
+ */
+static inline void MessageHandling_setErrorVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _errorVisibility )
+{
+ 	_THIS->errorVisibility = _errorVisibility;
+}
+
+
+/*
+ *	s e t W a r n i n g V i s i b i l i t y S t a t u s
+ */
+static inline void MessageHandling_setWarningVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _warningVisibility )
+{
+ 	_THIS->warningVisibility = _warningVisibility;
+}
+
+
+/*
+ *	s e t I n f o V i s i b i l i t y S t a t u s
+ */
+static inline void MessageHandling_setInfoVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _infoVisibility )
+{
+ 	_THIS->infoVisibility = _infoVisibility;
+}
+
+
+/*
+ *	s e t O u t p u t F i l e
+ */
+static inline void MessageHandling_setOutputFile( MessageHandling* _THIS, FILE* _outputFile )
+{
+ 	_THIS->outputFile = _outputFile;
+}
+
+
+/*
+ *	s e t E r r o r C o u n t
+ */
+static inline returnValue MessageHandling_setErrorCount( MessageHandling* _THIS, int _errorCount )
+{
+	if ( _errorCount >= 0 )
+	{
+		_THIS->errorCount = _errorCount;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return RET_INVALID_ARGUMENTS;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif /* QPOASES_MESSAGEHANDLING_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Options.h b/third_party/acados/include/qpOASES_e/Options.h
new file mode 100644
index 000000000..b471ee066
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Options.h
@@ -0,0 +1,153 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Options.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the Options class designed to manage user-specified
+ *	options for solving a QProblem.
+ */
+
+
+#ifndef QPOASES_OPTIONS_H
+#define QPOASES_OPTIONS_H
+
+
+#include <qpOASES_e/Utils.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/**
+ *	\brief Manages all user-specified options for solving QPs.
+ *
+ *	This class manages all user-specified options used for solving
+ *	quadratic programs.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	PrintLevel printLevel;					/**< Print level. */
+
+	BooleanType enableRamping;				/**< Specifies whether ramping shall be enabled or not. */
+	BooleanType enableFarBounds;			/**< Specifies whether far bounds shall be used or not. */
+	BooleanType enableFlippingBounds;		/**< Specifies whether flipping bounds shall be used or not. */
+	BooleanType enableRegularisation;		/**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */
+	BooleanType enableFullLITests;			/**< Specifies whether condition-hardened LI test shall be used or not. */
+	BooleanType enableNZCTests;				/**< Specifies whether nonzero curvature tests shall be used. */
+	int enableDriftCorrection;				/**< Specifies the frequency of drift corrections (0 = off). */
+	int enableCholeskyRefactorisation;		/**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */
+	BooleanType enableEqualities;			/**< Specifies whether equalities shall be always treated as active constraints. */
+
+	real_t terminationTolerance;			/**< Termination tolerance. */
+	real_t boundTolerance;					/**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */
+	real_t boundRelaxation;					/**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */
+	real_t epsNum;							/**< Numerator tolerance for ratio tests. */
+	real_t epsDen;							/**< Denominator tolerance for ratio tests. */
+	real_t maxPrimalJump;					/**< Maximum allowed jump in primal variables in nonzero curvature tests. */
+	real_t maxDualJump;						/**< Maximum allowed jump in dual variables in linear independence tests. */
+
+	real_t initialRamping;					/**< Start value for Ramping Strategy. */
+	real_t finalRamping;					/**< Final value for Ramping Strategy. */
+	real_t initialFarBounds;				/**< Initial size of Far Bounds. */
+	real_t growFarBounds;					/**< Factor to grow Far Bounds. */
+	SubjectToStatus initialStatusBounds;	/**< Initial status of bounds at first iteration. */
+	real_t epsFlipping;						/**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */
+	int numRegularisationSteps;				/**< Maximum number of successive regularisation steps. */
+	real_t epsRegularisation;				/**< Scaling factor of identity matrix used for Hessian regularisation. */
+	int numRefinementSteps;					/**< Maximum number of iterative refinement steps. */
+	real_t epsIterRef;						/**< Early termination tolerance for iterative refinement. */
+	real_t epsLITests;						/**< Tolerance for linear independence tests. */
+	real_t epsNZCTests;						/**< Tolerance for nonzero curvature tests. */
+
+	BooleanType enableDropInfeasibles;		/**< ... */
+	int    dropBoundPriority;				/**< ... */
+	int    dropEqConPriority;				/**< ... */
+	int    dropIneqConPriority;				/**< ... */
+} Options;
+
+
+void OptionsCON(	Options* _THIS
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void OptionsCPY(	Options* FROM,
+					Options* TO
+					);
+
+
+/** Sets all options to default values.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToDefault(	Options* _THIS
+									);
+
+/** Sets all options to values resulting in maximum reliabilty.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToReliable(	Options* _THIS
+									);
+
+/** Sets all options to values resulting in minimum solution time.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToMPC(		Options* _THIS
+									);
+
+/** Same as setToMPC( ), for ensuring backwards compatibility.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_setToFast(		Options* _THIS
+									);
+
+
+/** Ensures that all options have consistent values by automatically
+ *	adjusting inconsistent ones.
+ *	Note: This routine cannot (and does not try to) ensure that values
+ *	      are set to reasonable values that make the QP solution work!
+ *	\return SUCCESSFUL_RETURN \n 
+ *		    RET_OPTIONS_ADJUSTED */
+returnValue Options_ensureConsistency(	Options* _THIS
+										);
+
+
+/** Prints values of all options.
+ *	\return SUCCESSFUL_RETURN */
+returnValue Options_print(	Options* _THIS
+							);
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_OPTIONS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/QProblem.h b/third_party/acados/include/qpOASES_e/QProblem.h
new file mode 100644
index 000000000..3c61a4d59
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/QProblem.h
@@ -0,0 +1,2369 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/QProblem.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the QProblem class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming.
+ */
+
+
+
+#ifndef QPOASES_QPROBLEM_H
+#define QPOASES_QPROBLEM_H
+
+
+#include <qpOASES_e/Bounds.h>
+#include <qpOASES_e/Options.h>
+#include <qpOASES_e/Matrices.h>
+#include <qpOASES_e/Constraints.h>
+#include <qpOASES_e/Flipper.h>
+#include <qpOASES_e/ConstraintProduct.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+typedef struct {
+	Bounds *auxiliaryBounds;
+	Constraints *auxiliaryConstraints;
+
+	real_t *ub_new_far;
+	real_t *lb_new_far;
+	real_t *ubA_new_far;
+	real_t *lbA_new_far;
+
+	real_t *g_new;
+	real_t *lb_new;
+	real_t *ub_new;
+	real_t *lbA_new;
+	real_t *ubA_new;
+
+	real_t *g_new2;
+	real_t *lb_new2;
+	real_t *ub_new2;
+	real_t *lbA_new2;
+	real_t *ubA_new2;
+
+	real_t *delta_xFX5;
+	real_t *delta_xFR5;
+	real_t *delta_yAC5;
+	real_t *delta_yFX5;
+
+	real_t *Hx;
+
+	real_t *_H;
+
+	real_t *g_original;
+	real_t *lb_original;
+	real_t *ub_original;
+	real_t *lbA_original;
+	real_t *ubA_original;
+
+	real_t *delta_xFR;
+	real_t *delta_xFX;
+	real_t *delta_yAC;
+	real_t *delta_yFX;
+	real_t *delta_g;
+	real_t *delta_lb;
+	real_t *delta_ub;
+	real_t *delta_lbA;
+	real_t *delta_ubA;
+
+	real_t *gMod;
+
+	real_t *aFR;
+	real_t *wZ;
+
+	real_t *delta_g2;
+	real_t *delta_xFX2;
+	real_t *delta_xFR2;
+	real_t *delta_yAC2;
+	real_t *delta_yFX2;
+	real_t *nul;
+	real_t *Arow;
+
+	real_t *xiC;
+	real_t *xiC_TMP;
+	real_t *xiB;
+	real_t *Arow2;
+	real_t *num;
+
+	real_t *w;
+	real_t *tmp;
+
+	real_t *delta_g3;
+	real_t *delta_xFX3;
+	real_t *delta_xFR3;
+	real_t *delta_yAC3;
+	real_t *delta_yFX3;
+	real_t *nul2;
+
+	real_t *xiC2;
+	real_t *xiC_TMP2;
+	real_t *xiB2;
+	real_t *num2;
+
+	real_t *Hz;
+	real_t *z;
+	real_t *ZHz;
+	real_t *r;
+
+	real_t *tmp2;
+	real_t *Hz2;
+	real_t *z2;
+	real_t *r2;
+	real_t *rhs;
+
+	real_t *delta_xFX4;
+	real_t *delta_xFR4;
+	real_t *delta_yAC4;
+	real_t *delta_yFX4;
+	real_t *nul3;
+	real_t *ek;
+	real_t *x_W;
+	real_t *As;
+	real_t *Ax_W;
+
+	real_t *num3;
+	real_t *den;
+	real_t *delta_Ax_l;
+	real_t *delta_Ax_u;
+	real_t *delta_Ax;
+	real_t *delta_x;
+
+	real_t *_A;
+
+	real_t *grad;
+	real_t *AX;
+} QProblem_ws;
+
+int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory );
+
+QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC );
+
+/**
+ *	\brief Implements the online active set strategy for QPs with general constraints.
+ *
+ *	A class for setting up and solving quadratic programs. The main feature is
+ *	the possibily to use the newly developed online active set strategy for
+ * 	parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	QProblem_ws *ws;				/**< Workspace */
+	Bounds *bounds;					/**< Data structure for problem's bounds. */
+	Constraints *constraints;		/**< Data structure for problem's constraints. */
+	Flipper *flipper;				/**< Struct for making a temporary copy of the matrix factorisations. */
+
+	DenseMatrix* H;					/**< Hessian matrix pointer. */
+	DenseMatrix* A;					/**< Constraint matrix pointer. */
+
+	Options options;				/**< Struct containing all user-defined options for solving QPs. */
+	TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
+
+	real_t *g;						/**< Gradient. */
+
+	real_t *lb;						/**< Lower bound vector (on variables). */
+	real_t *ub;						/**< Upper bound vector (on variables). */
+	real_t *lbA;					/**< Lower constraints' bound vector. */
+	real_t *ubA;					/**< Upper constraints' bound vector. */
+
+	real_t *R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
+
+	real_t *T;						/**< Reverse triangular matrix, A = [0 T]*Q'. */
+	real_t *Q;						/**< Orthonormal quadratic matrix, A = [0 T]*Q'. */
+
+	real_t *Ax;						/**< Stores the current A*x \n
+									 *	 (for increased efficiency only). */
+	real_t *Ax_l;					/**< Stores the current distance to lower constraints' bounds A*x-lbA \n
+									 *	 (for increased efficiency only). */
+	real_t *Ax_u;					/**< Stores the current distance to lower constraints' bounds ubA-A*x \n
+									 *	 (for increased efficiency only). */
+
+	real_t *x;						/**< Primal solution vector. */
+	real_t *y;						/**< Dual solution vector. */
+
+	real_t *delta_xFR_TMP;			/**< Temporary for determineStepDirection */
+	real_t *tempA;					/**< Temporary for determineStepDirection. */
+	real_t *tempB;					/**< Temporary for determineStepDirection. */
+	real_t *ZFR_delta_xFRz;			/**< Temporary for determineStepDirection. */
+	real_t *delta_xFRy;				/**< Temporary for determineStepDirection. */
+	real_t *delta_xFRz;				/**< Temporary for determineStepDirection. */
+	real_t *delta_yAC_TMP;			/**< Temporary for determineStepDirection. */
+
+	ConstraintProduct constraintProduct;	/**< Pointer to user-defined constraint product function. */
+
+	real_t tau;						/**< Last homotopy step length. */
+	real_t regVal;					/**< Holds the offset used to regularise Hessian matrix (zero by default). */
+
+	real_t ramp0;					/**< Start value for Ramping Strategy. */
+	real_t ramp1;					/**< Final value for Ramping Strategy. */
+
+	QProblemStatus status;			/**< Current status of the solution process. */
+	HessianType hessianType;		/**< Type of Hessian matrix. */
+
+	BooleanType haveCholesky;		/**< Flag indicating whether Cholesky decomposition has already been setup. */
+	BooleanType infeasible;			/**< QP infeasible? */
+	BooleanType unbounded;			/**< QP unbounded? */
+
+	int rampOffset;					/**< Offset index for Ramping. */
+	unsigned int count;				/**< Counts the number of hotstart function calls (internal usage only!). */
+
+	int sizeT;						/**< Matrix T is stored in a (sizeT x sizeT) array. */
+} QProblem;
+
+int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory );
+
+QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC );
+
+
+/** Constructor which takes the QP dimension and Hessian type
+ *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+ *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+ *  is allocated for it and a NULL pointer can be passed for it
+ *  to the init() functions. */
+void QProblemCON(	QProblem* _THIS,
+					int _nV,	  					/**< Number of variables. */
+					int _nC,		  				/**< Number of constraints. */
+					HessianType _hessianType 		/**< Type of Hessian matrix. */
+					);
+
+/** Copies all members from given rhs object.
+ *  \return SUCCESSFUL_RETURN */
+void QProblemCPY(	QProblem* FROM,
+					QProblem* TO
+					);
+
+
+/** Clears all data structures of QProblem except for QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_RESET_FAILED */
+returnValue QProblem_reset( QProblem* _THIS );
+
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initM(	QProblem* _THIS,
+							DenseMatrix *_H,			/**< Hessian matrix. */
+							const real_t* const _g, 	/**< Gradient vector. */
+							DenseMatrix *_A,  			/**< Constraint matrix. */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_init(	QProblem* _THIS,
+							real_t* const _H, 			/**< Hessian matrix. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 	/**< Gradient vector. */
+							real_t* const _A,  			/**< Constraint matrix. */
+							const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,	/**< Upper constraints' bound vector. \n
+															 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+/** Initialises a QP problem with given QP data to be read from files and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblem_initF(	QProblem* _THIS,
+							const char* const H_file,	/**< Name of file where Hessian matrix is stored. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const char* const g_file,	/**< Name of file where gradient vector is stored. */
+							const char* const A_file,	/**< Name of file where constraint matrix is stored. */
+							const char* const lb_file,	/**< Name of file where lower bound vector. \n
+															If no lower bounds exist, a NULL pointer can be passed. */
+							const char* const ub_file,	/**< Name of file where upper bound vector. \n
+															If no upper bounds exist, a NULL pointer can be passed. */
+							const char* const lbA_file,	/**< Name of file where lower constraints' bound vector. \n
+															If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const char* const ubA_file,	/**< Name of file where upper constraints' bound vector. \n
+															 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,					/**< Input: Maximum number of working set recalculations when using initial homotopy.
+															 Output: Number of performed working set recalculations. */
+							real_t* const cputime	 	/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initMW(	QProblem* _THIS,
+								DenseMatrix *_H, 						/**< Hessian matrix. \n
+														    				 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const real_t* const _g, 				/**< Gradient vector. */
+								DenseMatrix *_A, 			 			/**< Constraint matrix. */
+								const real_t* const _lb,				/**< Lower bound vector (on variables). \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ub,				/**< Upper bound vector (on variables). \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input: Maximum number of working set recalculations when using initial homotopy.
+																		 *	 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+								const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																	 		 The Cholesky factor must be stored in a real_t array of size nV*nV
+																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Initialises a QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_initW(	QProblem* _THIS,
+							real_t* const _H, 						/**< Hessian matrix. \n
+														    			 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g, 				/**< Gradient vector. */
+							real_t* const _A,  						/**< Constraint matrix. */
+							const real_t* const _lb,				/**< Lower bound vector (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,				/**< Upper bound vector (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+							const real_t* const _lbA,				/**< Lower constraints' bound vector. \n
+																		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ubA,				/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+							int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
+																	 *	 Output: Number of performed working set recalculations. */
+							real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
+																		 Output: CPU time spent for QP initialisation. */
+							const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																		 (If a null pointer is passed, the old primal solution is kept!) */
+							const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																		 (If a null pointer is passed, the old dual solution is kept!) */
+							Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+							Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+							const real_t* const _R					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																	 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																		 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																		 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+							);
+
+/** Initialises a QP problem with given QP data to be ream from files and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0    : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0    : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n
+ *	3. 0,    yOpt, 0    : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	4. 0,    0,    gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n
+ *	5. xOpt, yOpt, 0    : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n
+ *	6. xOpt, 0,    gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n
+ *	7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblem_initFW(	QProblem* _THIS,
+								const char* const H_file,				/**< Name of file where Hessian matrix is stored. \n
+														     				 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const char* const g_file,				/**< Name of file where gradient vector is stored. */
+								const char* const A_file,				/**< Name of file where constraint matrix is stored. */
+								const char* const lb_file,				/**< Name of file where lower bound vector. \n
+																			 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file,				/**< Name of file where upper bound vector. \n
+																			 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file,				/**< Name of file where lower constraints' bound vector. \n
+																			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file,				/**< Name of file where upper constraints' bound vector. \n
+																			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input:  Maximum number of working set recalculations when using initial homotopy.
+																			 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input:  Maximum CPU time allowed for QP initialisation. \n
+																			 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,				/**< Optimal primal solution vector. \n
+																			 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,				/**< Optimal dual solution vector. \n
+																			 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+								Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+								const char* const R_file				/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																		 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																			 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																			 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_hotstart(	QProblem* _THIS,
+								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,						/**< Input:  Maximum number of working set recalculations; \n
+													 				 Output: Number of performed working set recalculations. */
+								real_t* const cputime 			/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *	where QP data is read from files. QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_hotstartF(	QProblem* _THIS,
+								const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no upper bounds exist, a NULL pointer can be passed. */
+								const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 					 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 					/**< Input:  Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime 		/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_SETUP_AUXILIARYQP_FAILED */
+returnValue QProblem_hotstartW(	QProblem* _THIS,
+								const real_t* const g_new,				/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,				/**< Lower bounds of neighbouring QP to be solved. \n
+											 						 		 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,				/**< Upper bounds of neighbouring QP to be solved. \n
+											 						 		 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,			/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 						 		 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,			/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 								 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
+																			 Output: Number of performed working set recalculations. */
+								real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 		 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																			 (If a null pointer is passed, the previous working set of bounds is kept!) */
+								Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
+																			 (If a null pointer is passed, the previous working set of constraints is kept!) */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *  where QP data is read from files.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_SETUP_AUXILIARYQP_FAILED \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_hotstartFW(	QProblem* _THIS,
+									const char* const g_file, 				/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+									const char* const lb_file, 				/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no lower bounds exist, a NULL pointer can be passed. */
+									const char* const ub_file, 				/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no upper bounds exist, a NULL pointer can be passed. */
+									const char* const lbA_file, 			/**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const char* const ubA_file, 			/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+											 									 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+									int* nWSR,								/**< Input: Maximum number of working set recalculations; \n
+																 				 Output: Number of performed working set recalculations. */
+									real_t* const cputime,					/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+									Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																				 (If a null pointer is passed, the previous working set of bounds is kept!) */
+									Constraints* const guessedConstraints	/**< Optimal working set of constraints for solution (xOpt,yOpt). \n
+																				 (If a null pointer is passed, the previous working set of constraints is kept!) */
+									);
+
+
+/** Solves using the current working set
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_STEPDIRECTION_FAILED_TQ \n
+ *			RET_STEPDIRECTION_FAILED_CHOLESKY \n
+ *			RET_INVALID_ARGUMENTS */
+returnValue QProblem_solveCurrentEQP (	QProblem* _THIS,
+										const int n_rhs,		/**< Number of consecutive right hand sides */
+										const real_t* g_in,		/**< Gradient of neighbouring QP to be solved. */
+										const real_t* lb_in,	/**< Lower bounds of neighbouring QP to be solved. \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* ub_in,	/**< Upper bounds of neighbouring QP to be solved. \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+										const real_t* lbA_in,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+										const real_t* ubA_in,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n */
+										real_t* x_out,			/**< Output: Primal solution */
+										real_t* y_out			/**< Output: Dual solution */
+										);
+
+
+
+/** Returns current constraints object of the QP (deep copy).
+  *	\return SUCCESSFUL_RETURN \n
+  			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_getConstraints(	QProblem* _THIS,
+													Constraints* _constraints	/** Output: Constraints object. */
+													);
+
+
+/** Returns the number of constraints.
+ *	\return Number of constraints. */
+static inline int QProblem_getNC( QProblem* _THIS );
+
+/** Returns the number of (implicitly defined) equality constraints.
+ *	\return Number of (implicitly defined) equality constraints. */
+static inline int QProblem_getNEC( QProblem* _THIS );
+
+/** Returns the number of active constraints.
+ *	\return Number of active constraints. */
+static inline int QProblem_getNAC( QProblem* _THIS );
+
+/** Returns the number of inactive constraints.
+ *	\return Number of inactive constraints. */
+static inline int QProblem_getNIAC( QProblem* _THIS );
+
+/** Returns the dimension of null space.
+ *	\return Dimension of null space. */
+int QProblem_getNZ( QProblem* _THIS );
+
+
+/** Returns the dual solution vector (deep copy).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblem_getDualSolution(	QProblem* _THIS,
+										real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
+										);
+
+
+/** Defines user-defined routine for calculating the constraint product A*x
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_setConstraintProduct(	QProblem* _THIS,
+											ConstraintProduct _constraintProduct
+											);
+
+
+/** Prints concise list of properties of the current QP.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printProperties( QProblem* _THIS );
+
+
+
+/** Writes a vector with the state of the working set
+*	\return SUCCESSFUL_RETURN */
+returnValue QProblem_getWorkingSet(	QProblem* _THIS,
+									real_t* workingSet	/** Output: array containing state of the working set. */
+									);
+
+/** Writes a vector with the state of the working set of bounds
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblem_getWorkingSetBounds(	QProblem* _THIS,
+											real_t* workingSetB	/** Output: array containing state of the working set of bounds. */
+											);
+
+/** Writes a vector with the state of the working set of constraints
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblem_getWorkingSetConstraints(	QProblem* _THIS,
+												real_t* workingSetC	/** Output: array containing state of the working set of constraints. */
+												);
+
+
+/** Returns current bounds object of the QP (deep copy).
+  *	\return SUCCESSFUL_RETURN \n
+  			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_getBounds(	QProblem* _THIS,
+												Bounds* _bounds		/** Output: Bounds object. */
+												);
+
+
+/** Returns the number of variables.
+ *	\return Number of variables. */
+static inline int QProblem_getNV( QProblem* _THIS );
+
+/** Returns the number of free variables.
+ *	\return Number of free variables. */
+static inline int QProblem_getNFR( QProblem* _THIS );
+
+/** Returns the number of fixed variables.
+ *	\return Number of fixed variables. */
+static inline int QProblem_getNFX( QProblem* _THIS );
+
+/** Returns the number of implicitly fixed variables.
+ *	\return Number of implicitly fixed variables. */
+static inline int QProblem_getNFV( QProblem* _THIS );
+
+
+/** Returns the optimal objective function value.
+ *	\return finite value: Optimal objective function value (QP was solved) \n
+ 			+infinity:	  QP was not yet solved */
+real_t QProblem_getObjVal( QProblem* _THIS );
+
+/** Returns the objective function value at an arbitrary point x.
+ *	\return Objective function value at point x */
+real_t QProblem_getObjValX(	QProblem* _THIS,
+							const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
+							);
+
+/** Returns the primal solution vector.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblem_getPrimalSolution(	QProblem* _THIS,
+										real_t* const xOpt	/**< Output: Primal solution vector (if QP has been solved). */
+										);
+
+
+/** Returns status of the solution process.
+ *	\return Status of solution process. */
+static inline QProblemStatus QProblem_getStatus( QProblem* _THIS );
+
+
+/** Returns if the QProblem object is initialised.
+ *	\return BT_TRUE:  QProblem initialised \n
+ 			BT_FALSE: QProblem not initialised */
+static inline BooleanType QProblem_isInitialised( QProblem* _THIS );
+
+/** Returns if the QP has been solved.
+ *	\return BT_TRUE:  QProblem solved \n
+ 			BT_FALSE: QProblem not solved */
+static inline BooleanType QProblem_isSolved( QProblem* _THIS );
+
+/** Returns if the QP is infeasible.
+ *	\return BT_TRUE:  QP infeasible \n
+ 			BT_FALSE: QP feasible (or not known to be infeasible!) */
+static inline BooleanType QProblem_isInfeasible( QProblem* _THIS );
+
+/** Returns if the QP is unbounded.
+ *	\return BT_TRUE:  QP unbounded \n
+ 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
+static inline BooleanType QProblem_isUnbounded( QProblem* _THIS );
+
+
+/** Returns Hessian type flag (type is not determined due to _THIS call!).
+ *	\return Hessian type. */
+static inline HessianType QProblem_getHessianType( QProblem* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setHessianType(	QProblem* _THIS,
+													HessianType _hessianType /**< New Hessian type. */
+													);
+
+/** Returns if the QP has been internally regularised.
+ *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
+ 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
+static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS );
+
+/** Returns current options struct.
+ *	\return Current options struct. */
+static inline Options QProblem_getOptions( QProblem* _THIS );
+
+/** Overrides current options with given ones.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setOptions(	QProblem* _THIS,
+												Options _options	/**< New options. */
+												);
+
+/** Returns the print level.
+ *	\return Print level. */
+static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setPrintLevel(	QProblem* _THIS,
+									PrintLevel _printlevel	/**< New print level. */
+									);
+
+
+/** Returns the current number of QP problems solved.
+ *	\return Number of QP problems solved. */
+static inline unsigned int QProblem_getCount( QProblem* _THIS );
+
+/** Resets QP problem counter (to zero).
+ *	\return SUCCESSFUL_RETURN. */
+static inline returnValue QProblem_resetCounter( QProblem* _THIS );
+
+
+/** Prints a list of all options and their current values.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printOptions( QProblem* _THIS );
+
+
+/** Solves a QProblem whose QP data is assumed to be stored in the member variables.
+ *  A guess for its primal/dual optimal solution vectors and the corresponding
+ *  working sets of bounds and constraints can be provided.
+ *  Note: This function is internally called by all init functions!
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_TQ \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED */
+returnValue QProblem_solveInitialQP(	QProblem* _THIS,
+										const real_t* const xOpt,				/**< Optimal primal solution vector.*/
+										const real_t* const yOpt,				/**< Optimal dual solution vector. */
+										Bounds* const guessedBounds,			/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+										Constraints* const guessedConstraints,	/**< Optimal working set of constraints for solution (xOpt,yOpt). */
+										const real_t* const _R,					/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */
+										int* nWSR, 								/**< Input:  Maximum number of working set recalculations; \n
+														 						 *	 Output: Number of performed working set recalculations. */
+										real_t* const cputime					/**< Input:  Maximum CPU time allowed for QP solution. \n
+																	 			 *	 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+										);
+
+/** Solves QProblem using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_solveQP(	QProblem* _THIS,
+								const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper bounds exist, a NULL pointer can be passed. */
+								const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+								const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+											 			 			 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+								int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
+													 				 Output: Number of performed working set recalculations. */
+								real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
+																	 this QP within previous solveQP() calls. This number is
+																	 always zero, except for successive calls from solveRegularisedQP()
+																	 or when using the far bound strategy. */
+								BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
+								);
+
+
+/** Solves QProblem using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_solveRegularisedQP(	QProblem* _THIS,
+											const real_t* const g_new,		/**< Gradient of neighbouring QP to be solved. */
+											const real_t* const lb_new,		/**< Lower bounds of neighbouring QP to be solved. \n
+													 			 				 If no lower bounds exist, a NULL pointer can be passed. */
+											const real_t* const ub_new,		/**< Upper bounds of neighbouring QP to be solved. \n
+													 			 				 If no upper bounds exist, a NULL pointer can be passed. */
+											const real_t* const lbA_new,	/**< Lower constraints' bounds of neighbouring QP to be solved. \n
+													 			 				 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const real_t* const ubA_new,	/**< Upper constraints' bounds of neighbouring QP to be solved. \n
+													 			 				 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											int* nWSR,						/**< Input: Maximum number of working set recalculations; \n
+															 					 Output: Number of performed working set recalculations. */
+											real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP solution. \n
+																 				 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+											int  nWSRperformed,				/**< Number of working set recalculations already performed to solve
+																				 this QP within previous solveRegularisedQP() calls. This number is
+																				 always zero, except for successive calls when using the far bound strategy. */
+											BooleanType isFirstCall			/**< Indicating whether this is the first call for current QP. */
+											);
+
+
+/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblem_setupSubjectToType( QProblem* _THIS );
+
+/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblem_setupSubjectToTypeNew(	QProblem* _THIS,
+											const real_t* const lb_new,		/**< New lower bounds. */
+											const real_t* const ub_new,		/**< New upper bounds. */
+											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
+											const real_t* const ubA_new		/**< New upper constraints' bounds. */
+											);
+
+/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z).
+ *  Note: If Hessian turns out not to be positive definite, the Hessian type
+ *		  is set to HST_SEMIDEF accordingly.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_computeProjectedCholesky( QProblem* _THIS );
+
+/** Computes initial Cholesky decomposition of the projected Hessian making
+ *  use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected().
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_setupInitialCholesky( QProblem* _THIS );
+
+/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_setupTQfactorisation( QProblem* _THIS );
+
+
+/** Obtains the desired working set for the auxiliary initial QP in
+ *  accordance with the user specifications
+ *  (assumes that member AX has already been initialised!)
+ *	\return SUCCESSFUL_RETURN \n
+			RET_OBTAINING_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
+												const real_t* const xOpt,					/**< Optimal primal solution vector.
+																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												const real_t* const yOpt,					/**< Optimal dual solution vector.
+																							 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+												Bounds* const guessedBounds,				/**< Guessed working set of bounds for solution (xOpt,yOpt). */
+												Constraints* const guessedConstraints,		/**< Guessed working set for solution (xOpt,yOpt). */
+												Bounds* auxiliaryBounds,					/**< Input: Allocated bound object. \n
+																							 *	 Ouput: Working set of constraints for auxiliary QP. */
+												Constraints* auxiliaryConstraints			/**< Input: Allocated bound object. \n
+																							 *	 Ouput: Working set for auxiliary QP. */
+												);
+
+/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints.
+ *  (If the working set shall be setup afresh, make sure that
+ *  bounds and constraints data structure have been resetted
+ *  and the TQ factorisation has been initialised!)
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUP_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNOWN_BUG */
+returnValue QProblem_setupAuxiliaryWorkingSet(	QProblem* _THIS,
+												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
+												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+												BooleanType setupAfresh						/**< Flag indicating if given working set shall be
+																							 *    setup afresh or by updating the current one. */
+												);
+
+/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setupAuxiliaryQPsolution(	QProblem* _THIS,
+												const real_t* const xOpt,	/**< Optimal primal solution vector.
+																		 	 *	 If a NULL pointer is passed, all entries are set to zero. */
+												const real_t* const yOpt	/**< Optimal dual solution vector.
+																			 *	 If a NULL pointer is passed, all entries are set to zero. */
+												);
+
+/** Sets up gradient of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS );
+
+/** Sets up (constraints') bounds of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNKNOWN_BUG */
+returnValue QProblem_setupAuxiliaryQPbounds(	QProblem* _THIS,
+												Bounds* const auxiliaryBounds,				/**< Working set of bounds for auxiliary QP. */
+												Constraints* const auxiliaryConstraints,	/**< Working set of constraints for auxiliary QP. */
+												BooleanType useRelaxation					/**< Flag indicating if inactive (constraints') bounds shall be relaxed. */
+												);
+
+
+/** Adds a constraint to active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDCONSTRAINT_FAILED \n
+			RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED */
+returnValue QProblem_addConstraint(	QProblem* _THIS,
+									int number,					/**< Number of constraint to be added to active set. */
+									SubjectToStatus C_status,	/**< Status of new active constraint. */
+									BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType ensureLI		/**< Ensure linear independence by exchange rules by default. */
+									);
+
+/** Checks if new active constraint to be added is linearly dependent from
+ *	from row of the active constraints matrix.
+ *	\return	 RET_LINEARLY_DEPENDENT \n
+ 			 RET_LINEARLY_INDEPENDENT \n
+			 RET_INDEXLIST_CORRUPTED */
+returnValue QProblem_addConstraint_checkLI(	QProblem* _THIS,
+											int number			/**< Number of constraint to be added to active set. */
+											);
+
+/** Ensures linear independence of constraint matrix when a new constraint is added.
+ * 	To _THIS end a bound or constraint is removed simultaneously if necessary.
+ *	\return	 SUCCESSFUL_RETURN \n
+ 			 RET_LI_RESOLVED \n
+			 RET_ENSURELI_FAILED \n
+			 RET_ENSURELI_FAILED_TQ \n
+			 RET_ENSURELI_FAILED_NOINDEX \n
+			 RET_REMOVE_FROM_ACTIVESET */
+returnValue QProblem_addConstraint_ensureLI(	QProblem* _THIS,
+												int number,					/**< Number of constraint to be added to active set. */
+												SubjectToStatus C_status	/**< Status of new active bound. */
+												);
+
+/** Adds a bound to active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDBOUND_FAILED \n
+			RET_ADDBOUND_FAILED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED */
+returnValue QProblem_addBound(	QProblem* _THIS,
+								int number,					/**< Number of bound to be added to active set. */
+								SubjectToStatus B_status,	/**< Status of new active bound. */
+								BooleanType updateCholesky,	/**< Flag indicating if Cholesky decomposition shall be updated. */
+								BooleanType ensureLI 		/**< Ensure linear independence by exchange rules by default. */
+								);
+
+/** Checks if new active bound to be added is linearly dependent from
+ *	from row of the active constraints matrix.
+ *	\return	 RET_LINEARLY_DEPENDENT \n
+ 			 RET_LINEARLY_INDEPENDENT */
+returnValue QProblem_addBound_checkLI(	QProblem* _THIS,
+										int number			/**< Number of bound to be added to active set. */
+										);
+
+/** Ensures linear independence of constraint matrix when a new bound is added.
+ *	To _THIS end a bound or constraint is removed simultaneously if necessary.
+ *	\return	 SUCCESSFUL_RETURN \n
+ 			 RET_LI_RESOLVED \n
+			 RET_ENSURELI_FAILED \n
+			 RET_ENSURELI_FAILED_TQ \n
+			 RET_ENSURELI_FAILED_NOINDEX \n
+			 RET_REMOVE_FROM_ACTIVESET */
+returnValue QProblem_addBound_ensureLI(	QProblem* _THIS,
+										int number,					/**< Number of bound to be added to active set. */
+										SubjectToStatus B_status	/**< Status of new active bound. */
+										);
+
+/** Removes a constraint from active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_CONSTRAINT_NOT_ACTIVE \n
+			RET_REMOVECONSTRAINT_FAILED \n
+			RET_HESSIAN_NOT_SPD */
+returnValue QProblem_removeConstraint(	QProblem* _THIS,
+										int number,						/**< Number of constraint to be removed from active set. */
+										BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
+										BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
+										BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+										);
+
+/** Removes a bounds from active set.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_BOUND_NOT_ACTIVE \n
+			RET_HESSIAN_NOT_SPD \n
+			RET_REMOVEBOUND_FAILED */
+returnValue QProblem_removeBound(	QProblem* _THIS,
+									int number,						/**< Number of bound to be removed from active set. */
+									BooleanType updateCholesky,		/**< Flag indicating if Cholesky decomposition shall be updated. */
+									BooleanType allowFlipping,		/**< Flag indicating if flipping bounds are allowed. */
+									BooleanType ensureNZC			/**< Flag indicating if non-zero curvature is ensured by exchange rules. */
+									);
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performPlainRatioTest(	QProblem* _THIS,
+											int nIdx, 					/**< Number of ratios to be checked. */
+											const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
+											const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
+											const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
+											real_t epsNum,				/**< Numerator tolerance. */
+											real_t epsDen,				/**< Denominator tolerance. */
+											real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
+											int* BC_idx 				/**< Output: Index of blocking constraint. */
+											);
+
+
+/** Ensure non-zero curvature by primal jump.
+ *  \return SUCCESSFUL_RETURN \n
+ *          RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblem_ensureNonzeroCurvature(	QProblem* _THIS,
+												BooleanType removeBoundNotConstraint,	/**< SubjectTo to be removed is a bound. */
+												int remIdx,								/**< Index of bound/constraint to be removed. */
+												BooleanType* exchangeHappened,			/**< Output: Exchange was necessary to ensure. */
+												BooleanType* addBoundNotConstraint,		/**< SubjectTo to be added is a bound. */
+												int* addIdx,							/**< Index of bound/constraint to be added. */
+												SubjectToStatus* addStatus				/**< Status of bound/constraint to be added. */
+												);
+
+
+/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveT(	QProblem* _THIS,
+									const real_t* const b,	/**< Right hand side vector. */
+									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+									real_t* const a 		/**< Output: Solution vector */
+									);
+
+
+/** Determines step direction of the shift of the QP data.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblem_determineDataShift(	QProblem* _THIS,
+											const real_t* const g_new,		/**< New gradient vector. */
+											const real_t* const lbA_new,	/**< New lower constraints' bounds. */
+											const real_t* const ubA_new,	/**< New upper constraints' bounds. */
+											const real_t* const lb_new,		/**< New lower bounds. */
+											const real_t* const ub_new,		/**< New upper bounds. */
+											real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
+											real_t* const delta_lbA,		/**< Output: Step direction of lower constraints' bounds. */
+											real_t* const delta_ubA,		/**< Output: Step direction of upper constraints' bounds. */
+											real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
+											real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
+											BooleanType* Delta_bC_isZero,	/**< Output: Indicates if active constraints' bounds are to be shifted. */
+											BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
+											);
+
+/** Determines step direction of the homotopy path.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_STEPDIRECTION_FAILED_TQ \n
+			RET_STEPDIRECTION_FAILED_CHOLESKY */
+returnValue QProblem_determineStepDirection(	QProblem* _THIS,
+												const real_t* const delta_g,	/**< Step direction of gradient vector. */
+												const real_t* const delta_lbA,	/**< Step direction of lower constraints' bounds. */
+												const real_t* const delta_ubA,	/**< Step direction of upper constraints' bounds. */
+												const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+												const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+												BooleanType Delta_bC_isZero, 	/**< Indicates if active constraints' bounds are to be shifted. */
+												BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
+												real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
+												real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
+												real_t* const delta_yAC, 		/**< Output: Dual homotopy step direction of active constraints' multiplier. */
+												real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
+												);
+
+/** Determines the maximum possible step length along the homotopy path
+ *  and performs _THIS step (without changing working set).
+ *	\return SUCCESSFUL_RETURN \n
+ * 			RET_ERROR_IN_CONSTRAINTPRODUCT \n
+ * 			RET_QP_INFEASIBLE */
+returnValue QProblem_performStep(	QProblem* _THIS,
+									const real_t* const delta_g,		/**< Step direction of gradient. */
+									const real_t* const delta_lbA,		/**< Step direction of lower constraints' bounds. */
+									const real_t* const delta_ubA,		/**< Step direction of upper constraints' bounds. */
+									const real_t* const delta_lb,	 	/**< Step direction of lower bounds. */
+									const real_t* const delta_ub,	 	/**< Step direction of upper bounds. */
+									const real_t* const delta_xFX, 		/**< Primal homotopy step direction of fixed variables. */
+									const real_t* const delta_xFR,		/**< Primal homotopy step direction of free variables. */
+									const real_t* const delta_yAC,		/**< Dual homotopy step direction of active constraints' multiplier. */
+									const real_t* const delta_yFX,		/**< Dual homotopy step direction of fixed variables' multiplier. */
+									int* BC_idx, 						/**< Output: Index of blocking constraint. */
+									SubjectToStatus* BC_status,			/**< Output: Status of blocking constraint. */
+									BooleanType* BC_isBound 			/**< Output: Indicates if blocking constraint is a bound. */
+									);
+
+/** Updates the active set.
+ *	\return  SUCCESSFUL_RETURN \n
+ 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
+			 RET_ADD_TO_ACTIVESET_FAILED */
+returnValue QProblem_changeActiveSet(	QProblem* _THIS,
+										int BC_idx, 				/**< Index of blocking constraint. */
+										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
+										BooleanType BC_isBound 		/**< Indicates if blocking constraint is a bound. */
+										);
+
+
+/** Compute relative length of homotopy in data space for termination
+ *  criterion.
+ *  \return Relative length in data space. */
+real_t QProblem_getRelativeHomotopyLength(	QProblem* _THIS,
+											const real_t* const g_new,		/**< Final gradient. */
+											const real_t* const lb_new,		/**< Final lower variable bounds. */
+											const real_t* const ub_new,		/**< Final upper variable bounds. */
+											const real_t* const lbA_new,	/**< Final lower constraint bounds. */
+											const real_t* const ubA_new		/**< Final upper constraint bounds. */
+											);
+
+
+/** Ramping Strategy to avoid ties. Modifies homotopy start without
+ *  changing current active set.
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblem_performRamping( QProblem* _THIS );
+
+
+/** ... */
+returnValue QProblem_updateFarBounds(	QProblem* _THIS,
+										real_t curFarBound,				/**< ... */
+										int nRamp,						/**< ... */
+										const real_t* const lb_new,		/**< ... */
+										real_t* const lb_new_far,		/**< ... */
+										const real_t* const ub_new,		/**< ... */
+										real_t* const ub_new_far,		/**< ... */
+										const real_t* const lbA_new,	/**< ... */
+										real_t* const lbA_new_far,		/**< ... */
+										const real_t* const ubA_new,	/**< ... */
+										real_t* const ubA_new_far		/**< ... */
+										);
+
+/** ... */
+returnValue QProblemBCPY_updateFarBounds(	QProblem* _THIS,
+											real_t curFarBound,				/**< ... */
+											int nRamp,						/**< ... */
+											const real_t* const lb_new,		/**< ... */
+											real_t* const lb_new_far,		/**< ... */
+											const real_t* const ub_new,		/**< ... */
+											real_t* const ub_new_far		/**< ... */
+											);
+
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performRatioTestC(	QProblem* _THIS,
+										int nIdx, 						/**< Number of ratios to be checked. */
+										const int* const idxList, 		/**< Array containing the indices of all ratios to be checked. */
+										Constraints* const subjectTo,	/**< Constraint object corresponding to ratios to be checked. */
+										const real_t* const num,	 	/**< Array containing all numerators for performing the ratio test. */
+										const real_t* const den,		/**< Array containing all denominators for performing the ratio test. */
+										real_t epsNum,					/**< Numerator tolerance. */
+										real_t epsDen,					/**< Denominator tolerance. */
+										real_t* t,						/**< Output: Maximum possible step length along the homotopy path. */
+										int* BC_idx 					/**< Output: Index of blocking constraint. */
+										);
+
+
+/** Drift correction at end of each active set iteration
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblem_performDriftCorrection( QProblem* _THIS );
+
+
+/** Updates QP vectors, working sets and internal data structures in order to
+	start from an optimal solution corresponding to initial guesses of the working
+	set for bounds and constraints.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_SETUP_AUXILIARYQP_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_setupAuxiliaryQP(	QProblem* _THIS,
+										Bounds* const guessedBounds,			/**< Initial guess for working set of bounds. */
+										Constraints* const guessedConstraints	/**< Initial guess for working set of constraints. */
+										);
+
+/** Determines if it is more efficient to refactorise the matrices when
+ *  hotstarting or not (i.e. better to update the existing factorisations).
+ *	\return BT_TRUE iff matrices shall be refactorised afresh
+ */
+BooleanType QProblem_shallRefactorise(	QProblem* _THIS,
+										Bounds* const guessedBounds,			/**< Guessed new working set of bounds. */
+										Constraints* const guessedConstraints	/**< Guessed new working set of constraints. */
+										);
+
+/** Setups internal QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdataM(	QProblem* _THIS,
+									DenseMatrix *_H, 			/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									DenseMatrix *_A, 			 /**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+
+/** Sets up dense internal QP data. If the current Hessian is trivial
+ *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdata(	QProblem* _THIS,
+									real_t* const _H, 			/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g, 	/**< Gradient vector. */
+									real_t* const _A,  			/**< Constraint matrix. */
+									const real_t* const _lb,	/**< Lower bound vector (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub,	/**< Upper bound vector (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									const real_t* const _lbA,	/**< Lower constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ubA	/**< Upper constraints' bound vector. \n
+																	 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+									);
+
+/** Sets up internal QP data by loading it from files. If the current Hessian
+ *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNONW_BUG */
+returnValue QProblem_setupQPdataFromFile(	QProblem* _THIS,
+											const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+														     				 If Hessian matrix is trivial,a NULL pointer can be passed. */
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const A_file,	/**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file	/**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											);
+
+/** Loads new QP vectors from files (internal members are not affected!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblem_loadQPvectorsFromFile(	QProblem* _THIS,
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
+											const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower constraints' bounds exist, a NULL pointer can be passed. */
+											const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper constraints' bounds exist, a NULL pointer can be passed. */
+											real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+											real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+											real_t* const ub_new,		/**< Output: Upper bounds of neighbouring QP to be solved */
+											real_t* const lbA_new,		/**< Output: Lower constraints' bounds of neighbouring QP to be solved */
+											real_t* const ubA_new		/**< Output: Upper constraints' bounds of neighbouring QP to be solved */
+											);
+
+
+/** Prints concise information on the current iteration.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblem_printIteration(	QProblem* _THIS,
+										int iter,					/**< Number of current iteration. */
+										int BC_idx, 				/**< Index of blocking constraint. */
+										SubjectToStatus BC_status,	/**< Status of blocking constraint. */
+										BooleanType BC_isBound,		/**< Indicates if blocking constraint is a bound. */
+										real_t homotopyLength,		/**< Current homotopy distance. */
+										BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
+ 										);
+
+
+/** Sets constraint matrix of the QP. \n
+	Note: Also internal vector Ax is recomputed!
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setAM(	QProblem* _THIS,
+											DenseMatrix *A_new	/**< New constraint matrix. */
+											);
+
+/** Sets dense constraint matrix of the QP. \n
+	Note: Also internal vector Ax is recomputed!
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setA(	QProblem* _THIS,
+											real_t* const A_new	/**< New dense constraint matrix (with correct dimension!). */
+											);
+
+
+/** Sets constraints' lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_setLBA(	QProblem* _THIS,
+											const real_t* const lbA_new	/**< New constraints' lower bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of lower constraints' bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setLBAn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of lower constraints' bound vector (with correct dimension!). */
+											);
+
+/** Sets constraints' upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblem_setUBA(	QProblem* _THIS,
+											const real_t* const ubA_new	/**< New constraints' upper bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of upper constraints' bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setUBAn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of upper constraints' bound vector (with correct dimension!). */
+											);
+
+
+/** Decides if lower bounds are smaller than upper bounds
+ *
+ * \return SUCCESSFUL_RETURN \n
+ * 		   RET_QP_INFEASIBLE */
+returnValue QProblem_areBoundsConsistent(	QProblem* _THIS,
+											const real_t* const lb,		/**< Vector of lower bounds*/
+											const real_t* const ub,		/**< Vector of upper bounds*/
+											const real_t* const lbA,	/**< Vector of lower constraints*/
+											const real_t* const ubA		/**< Vector of upper constraints*/
+											);
+
+
+/** Drops the blocking bound/constraint that led to infeasibility, or finds another
+ *  bound/constraint to drop according to drop priorities.
+ *  \return SUCCESSFUL_RETURN \n
+ */
+returnValue QProblem_dropInfeasibles ( 	QProblem* _THIS,
+										int BC_number,				/**< Number of the bound or constraint to be added */
+										SubjectToStatus BC_status, 	/**< New status of the bound or constraint to be added */
+										BooleanType BC_isBound,		/**< Whether a bound or a constraint is to be added */
+										real_t *xiB,
+										real_t *xiC
+										);
+
+
+/** If Hessian type has been set by the user, nothing is done.
+ *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
+ *  HST_POSDEF (default), respectively.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_INDEFINITE */
+returnValue QProblem_determineHessianType( QProblem* _THIS );
+
+/** Computes the Cholesky decomposition of the (simply projected) Hessian
+ *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
+ *  projection matrix!
+ *  Note: If Hessian turns out not to be positive definite, the Hessian type
+ *		  is set to HST_SEMIDEF accordingly.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblemBCPY_computeCholesky( QProblem* _THIS );
+
+/** Obtains the desired working set for the auxiliary initial QP in
+ *  accordance with the user specifications
+ *	\return SUCCESSFUL_RETURN \n
+			RET_OBTAINING_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_obtainAuxiliaryWorkingSet(	QProblem* _THIS,
+													const real_t* const xOpt,		/**< Optimal primal solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													const real_t* const yOpt,		/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
+													Bounds* auxiliaryBounds			/**< Input:  Allocated bound object. \n
+																					 *	 Output: Working set for auxiliary QP. */
+													);
+
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveR(	QProblem* _THIS,
+									const real_t* const b,	/**< Right hand side vector. */
+									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+									real_t* const a 		/**< Output: Solution vector */
+									);
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
+ *  Special variant for the case that _THIS function is called from within "removeBound()".
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblem_backsolveRrem(	QProblem* _THIS,
+									const real_t* const b,		/**< Right hand side vector. */
+									BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
+									BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
+									real_t* const a 			/**< Output: Solution vector */
+									);
+
+
+/** Determines step direction of the shift of the QP data.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemBCPY_determineDataShift(	QProblem* _THIS,
+												const real_t* const g_new,		/**< New gradient vector. */
+												const real_t* const lb_new,		/**< New lower bounds. */
+												const real_t* const ub_new,		/**< New upper bounds. */
+												real_t* const delta_g,	 		/**< Output: Step direction of gradient vector. */
+												real_t* const delta_lb,	 		/**< Output: Step direction of lower bounds. */
+												real_t* const delta_ub,	 		/**< Output: Step direction of upper bounds. */
+												BooleanType* Delta_bB_isZero	/**< Output: Indicates if active bounds are to be shifted. */
+												);
+
+
+/** Sets up internal QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_setupQPdataM(	QProblem* _THIS,
+										DenseMatrix *_H,	 		/**< Hessian matrix.*/
+										const real_t* const _g,		/**< Gradient vector. */
+										const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const _ub		/**< Upper bounds (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+										);
+
+/** Sets up internal QP data. If the current Hessian is trivial
+ *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemBCPY_setupQPdata(	QProblem* _THIS,
+										real_t* const _H, 			/**< Hessian matrix. \n
+																		 If Hessian matrix is trivial,a NULL pointer can be passed. */
+										const real_t* const _g,		/**< Gradient vector. */
+										const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																		 If no lower bounds exist, a NULL pointer can be passed. */
+										const real_t* const _ub		/**< Upper bounds (on variables). \n
+																		 If no upper bounds exist, a NULL pointer can be passed. */
+										);
+
+/** Sets up internal QP data by loading it from files. If the current Hessian
+ *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemBCPY_setupQPdataFromFile(	QProblem* _THIS,
+												const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+														     					 If Hessian matrix is trivial,a NULL pointer can be passed. */
+												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
+												const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
+												);
+
+/** Loads new QP vectors from files (internal members are not affected!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemBCPY_loadQPvectorsFromFile(	QProblem* _THIS,
+												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
+												const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
+												real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+												real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+												real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
+												);
+
+
+/** Sets internal infeasibility flag and throws given error in case the far bound
+ *	strategy is not enabled (as QP might actually not be infeasible in _THIS case).
+ *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED_CYCLING \n
+			RET_ENSURELI_FAILED_NOINDEX */
+returnValue QProblem_setInfeasibilityFlag(	QProblem* _THIS,
+											returnValue returnvalue,	/**< Returnvalue to be tunneled. */
+											BooleanType doThrowError	/**< Flag forcing to throw an error. */
+											);
+
+
+/** Determines if next QP iteration can be performed within given CPU time limit.
+ *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
+			BT_FALSE: Sufficient CPU time for next QP iteration. */
+BooleanType QProblem_isCPUtimeLimitExceeded(	QProblem* _THIS,
+												const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
+												real_t starttime,				/**< Start time of current QP solution. */
+												int nWSR						/**< Number of working set recalculations performed so far. */
+												);
+
+
+/** Regularise Hessian matrix by adding a scaled identity matrix to it.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_ALREADY_REGULARISED */
+returnValue QProblem_regulariseHessian( QProblem* _THIS );
+
+
+/** Sets Hessian matrix of the QP.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setHM(	QProblem* _THIS,
+											DenseMatrix* H_new	/**< New Hessian matrix. */
+											);
+
+/** Sets dense Hessian matrix of the QP.
+ *  If a null pointer is passed and
+ *  a) hessianType is HST_IDENTITY, nothing is done,
+ *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblem_setH(	QProblem* _THIS,
+											real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!). */
+											);
+
+/** Changes gradient vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setG(	QProblem* _THIS,
+											const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
+											);
+
+/** Changes lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setLB(	QProblem* _THIS,
+											const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setLBn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of lower bound vector. */
+											);
+
+/** Changes upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblem_setUB(	QProblem* _THIS,
+											const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblem_setUBn(	QProblem* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of upper bound vector. */
+											);
+
+
+
+/** Compute relative length of homotopy in data space for termination
+ *  criterion.
+ *  \return Relative length in data space. */
+real_t QProblemBCPY_getRelativeHomotopyLength(	QProblem* _THIS,
+												const real_t* const g_new,	/**< Final gradient. */
+												const real_t* const lb_new,	/**< Final lower variable bounds. */
+												const real_t* const ub_new	/**< Final upper variable bounds. */
+												);
+
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblem_performRatioTestB(	QProblem* _THIS,
+										int nIdx, 					/**< Number of ratios to be checked. */
+										const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
+										Bounds* const subjectTo,	/**< Bound object corresponding to ratios to be checked. */
+										const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
+										const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
+										real_t epsNum,				/**< Numerator tolerance. */
+										real_t epsDen,				/**< Denominator tolerance. */
+										real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
+										int* BC_idx 				/**< Output: Index of blocking constraint. */
+										);
+
+/** Checks whether given ratio is blocking, i.e. limits the maximum step length
+ *  along the homotopy path to a value lower than given one.
+ *	\return  SUCCESSFUL_RETURN */
+static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
+												real_t num,			/**< Numerator for performing the ratio test. */
+												real_t den,			/**< Denominator for performing the ratio test. */
+												real_t epsNum,		/**< Numerator tolerance. */
+												real_t epsDen,		/**< Denominator tolerance. */
+												real_t* t			/**< Input:  Current maximum step length along the homotopy path,
+																	 *   Output: Updated maximum possible step length along the homotopy path. */
+												);
+
+
+/** ...
+ *	\return SUCCESSFUL_RETURN  \n
+			RET_UNABLE_TO_OPEN_FILE */
+returnValue QProblem_writeQpDataIntoMatFile(	QProblem* _THIS,
+												const char* const filename	/**< Mat file name. */
+												);
+
+/** ...
+*	\return SUCCESSFUL_RETURN  \n
+			RET_UNABLE_TO_OPEN_FILE */
+returnValue QProblem_writeQpWorkspaceIntoMatFile(	QProblem* _THIS,
+													const char* const filename	/**< Mat file name. */
+													);
+
+
+/*
+ *	g e t B o u n d s
+ */
+static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	_bounds = _THIS->bounds;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N V
+ */
+static inline int QProblem_getNV( QProblem* _THIS )
+{
+	return Bounds_getNV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F R
+ */
+static inline int QProblem_getNFR( QProblem* _THIS )
+{
+	return Bounds_getNFR( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F X
+ */
+static inline int QProblem_getNFX( QProblem* _THIS )
+{
+	return Bounds_getNFX( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F V
+ */
+static inline int QProblem_getNFV( QProblem* _THIS )
+{
+	return Bounds_getNFV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+static inline QProblemStatus QProblem_getStatus( QProblem* _THIS )
+{
+	return _THIS->status;
+}
+
+
+/*
+ *	i s I n i t i a l i s e d
+ */
+static inline BooleanType QProblem_isInitialised( QProblem* _THIS )
+{
+	if ( _THIS->status == QPS_NOTINITIALISED )
+		return BT_FALSE;
+	else
+		return BT_TRUE;
+}
+
+
+/*
+ *	i s S o l v e d
+ */
+static inline BooleanType QProblem_isSolved( QProblem* _THIS )
+{
+	if ( _THIS->status == QPS_SOLVED )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	i s I n f e a s i b l e
+ */
+static inline BooleanType QProblem_isInfeasible( QProblem* _THIS )
+{
+	return _THIS->infeasible;
+}
+
+
+/*
+ *	i s U n b o u n d e d
+ */
+static inline BooleanType QProblem_isUnbounded( QProblem* _THIS )
+{
+	return _THIS->unbounded;
+}
+
+
+/*
+ *	g e t H e s s i a n T y p e
+ */
+static inline HessianType QProblem_getHessianType( QProblem* _THIS )
+{
+	return _THIS->hessianType;
+}
+
+
+/*
+ *	s e t H e s s i a n T y p e
+ */
+static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType )
+{
+	_THIS->hessianType = _hessianType;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	u s i n g R e g u l a r i s a t i o n
+ */
+static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS )
+{
+	if ( _THIS->regVal > QPOASES_ZERO )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	g e t O p t i o n s
+ */
+static inline Options QProblem_getOptions( QProblem* _THIS )
+{
+	return _THIS->options;
+}
+
+
+/*
+ *	s e t O p t i o n s
+ */
+static inline returnValue QProblem_setOptions(	QProblem* _THIS,
+												Options _options
+												)
+{
+	OptionsCPY( &_options,&(_THIS->options) );
+	Options_ensureConsistency( &(_THIS->options) );
+
+	QProblem_setPrintLevel( _THIS,_THIS->options.printLevel );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t P r i n t L e v e l
+ */
+static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS )
+{
+	return _THIS->options.printLevel;
+}
+
+
+/*
+ *	g e t C o u n t
+ */
+static inline unsigned int QProblem_getCount( QProblem* _THIS )
+{
+	return _THIS->count;
+}
+
+
+/*
+ *	r e s e t C o u n t e r
+ */
+static inline returnValue QProblem_resetCounter( QProblem* _THIS )
+{
+	_THIS->count = 0;
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new )
+{
+	if ( H_new == 0 )
+		return QProblem_setH( _THIS,(real_t*)0 );
+	else
+		return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) );
+}
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new )
+{
+	/* if null pointer is passed, Hessian is set to zero matrix
+	 *                            (or stays identity matrix) */
+	if ( H_new == 0 )
+	{
+		if ( _THIS->hessianType == HST_IDENTITY )
+			return SUCCESSFUL_RETURN;
+
+		_THIS->hessianType = HST_ZERO;
+
+		_THIS->H = 0;
+	}
+	else
+	{
+		DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t G
+ */
+static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new )
+{
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( g_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	memcpy( _THIS->g,g_new,nV*sizeof(real_t) );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lb_new != 0 )
+	{
+		memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower bounds are specified, set them to -infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->lb[i] = -QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->lb[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ub_new != 0 )
+	{
+		memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper bounds are specified, set them to infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->ub[i] = QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->ub[number] = value;
+
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+
+/*
+ * i s B l o c k i n g
+ */
+static inline BooleanType QProblem_isBlocking(	QProblem* _THIS,
+												real_t num,
+												real_t den,
+												real_t epsNum,
+												real_t epsDen,
+												real_t* t
+												)
+{
+	if ( ( den >= epsDen ) && ( num >= epsNum ) )
+	{
+		if ( num < (*t)*den )
+			return BT_TRUE;
+	}
+
+	return BT_FALSE;
+}
+
+
+
+/*
+ *	g e t C o n s t r a i n t s
+ */
+static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints )
+{
+	int nV = QProblem_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	ConstraintsCPY( _THIS->constraints,_constraints );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*
+ *	g e t N C
+ */
+static inline int QProblem_getNC( QProblem* _THIS )
+{
+	return Constraints_getNC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N E C
+ */
+static inline int QProblem_getNEC( QProblem* _THIS )
+{
+	return Constraints_getNEC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N A C
+ */
+static inline int QProblem_getNAC( QProblem* _THIS )
+{
+	return Constraints_getNAC( _THIS->constraints );
+}
+
+
+/*
+ *	g e t N I A C
+ */
+static inline int QProblem_getNIAC( QProblem* _THIS )
+{
+	return Constraints_getNIAC( _THIS->constraints );
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t A
+ */
+static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new )
+{
+	if ( A_new == 0 )
+		return QProblem_setA( _THIS,(real_t*)0 );
+	else
+		return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) );
+}
+
+
+/*
+ *	s e t A
+ */
+static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new )
+{
+	int j;
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( A_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new );
+
+	DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC);
+
+	for( j=0; j<nC; ++j )
+	{
+		_THIS->Ax_u[j] = _THIS->ubA[j] - _THIS->Ax[j];
+		_THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j];
+
+		/* (ckirches) disable constraints with empty rows */
+		if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE )
+			Constraints_setType( _THIS->constraints,j,ST_DISABLED );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lbA_new != 0 )
+	{
+		memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower constraints' bounds are specified, set them to -infinity */
+		for( i=0; i<nC; ++i )
+			_THIS->lbA[i] = -QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B A
+ */
+static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		_THIS->lbA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+/*
+ *	s e t U B A
+ */
+static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblem_getNV( _THIS );
+	unsigned int nC = (unsigned int)QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ubA_new != 0 )
+	{
+		memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper constraints' bounds are specified, set them to infinity */
+		for( i=0; i<nC; ++i )
+			_THIS->ubA[i] = QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B A
+ */
+static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value )
+{
+	int nV = QProblem_getNV( _THIS );
+	int nC = QProblem_getNC( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nC ) )
+	{
+		_THIS->ubA[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_QPROBLEM_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/QProblemB.h b/third_party/acados/include/qpOASES_e/QProblemB.h
new file mode 100644
index 000000000..ee5157dda
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/QProblemB.h
@@ -0,0 +1,1641 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/QProblemB.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of the QProblemB class which is able to use the newly
+ *	developed online active set strategy for parametric quadratic programming
+ *	for problems with (simple) bounds only.
+ */
+
+
+
+#ifndef QPOASES_QPROBLEMB_H
+#define QPOASES_QPROBLEMB_H
+
+
+#include <qpOASES_e/Bounds.h>
+#include <qpOASES_e/Options.h>
+#include <qpOASES_e/Matrices.h>
+#include <qpOASES_e/Flipper.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+typedef struct {
+	Bounds *emptyBounds;
+	Bounds *auxiliaryBounds;
+
+	real_t *ub_new_far;
+	real_t *lb_new_far;
+
+	real_t *g_new;
+	real_t *lb_new;
+	real_t *ub_new;
+
+	real_t *g_new2;
+	real_t *lb_new2;
+	real_t *ub_new2;
+
+	real_t *Hx;
+
+	real_t *_H;
+
+	real_t *g_original;
+	real_t *lb_original;
+	real_t *ub_original;
+
+	real_t *delta_xFR;
+	real_t *delta_xFX;
+	real_t *delta_yFX;
+	real_t *delta_g;
+	real_t *delta_lb;
+	real_t *delta_ub;
+
+	real_t *gMod;
+
+	real_t *num;
+	real_t *den;
+
+	real_t *rhs;
+	real_t *r;
+} QProblemB_ws;
+
+int QProblemB_ws_calculateMemorySize( unsigned int nV );
+
+char *QProblemB_ws_assignMemory( unsigned int nV, QProblemB_ws **mem, void *raw_memory );
+
+QProblemB_ws *QProblemB_ws_createMemory( unsigned int nV );
+
+
+/**
+ *	\brief Implements the online active set strategy for box-constrained QPs.
+ *
+ *	Class for setting up and solving quadratic programs with bounds (= box constraints) only.
+ *	The main feature is the possibility to use the newly developed online active set strategy
+ *	for parametric quadratic programming.
+ *
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ */
+typedef struct
+{
+	QProblemB_ws *ws;
+	Bounds *bounds;					/**< Data structure for problem's bounds. */
+	Flipper *flipper;				/**< Struct for making a temporary copy of the matrix factorisations. */
+
+	DenseMatrix* H;					/**< Hessian matrix pointer. */
+
+	Options options;				/**< Struct containing all user-defined options for solving QPs. */
+	TabularOutput tabularOutput;	/**< Struct storing information for tabular output (printLevel == PL_TABULAR). */
+
+	real_t *g;						/**< Gradient. */
+	real_t *lb;						/**< Lower bound vector (on variables). */
+	real_t *ub;						/**< Upper bound vector (on variables). */
+
+	real_t *R;						/**< Cholesky factor of H (i.e. H = R^T*R). */
+
+	real_t *x;						/**< Primal solution vector. */
+	real_t *y;						/**< Dual solution vector. */
+
+	real_t *delta_xFR_TMP;			/**< Temporary for determineStepDirection */
+
+	real_t tau;						/**< Last homotopy step length. */
+	real_t regVal;					/**< Holds the offset used to regularise Hessian matrix (zero by default). */
+
+	real_t ramp0;					/**< Start value for Ramping Strategy. */
+	real_t ramp1;					/**< Final value for Ramping Strategy. */
+
+	QProblemStatus status;			/**< Current status of the solution process. */
+	HessianType hessianType;		/**< Type of Hessian matrix. */
+
+	BooleanType haveCholesky;		/**< Flag indicating whether Cholesky decomposition has already been setup. */
+	BooleanType infeasible;			/**< QP infeasible? */
+	BooleanType unbounded;			/**< QP unbounded? */
+
+	int rampOffset;					/**< Offset index for Ramping. */
+	unsigned int count;				/**< Counts the number of hotstart function calls (internal usage only!). */
+} QProblemB;
+
+int QProblemB_calculateMemorySize( unsigned int nV );
+
+char *QProblemB_assignMemory( unsigned int nV, QProblemB **mem, void *raw_memory );
+
+QProblemB *QProblemB_createMemory( unsigned int nV );
+
+
+/** Constructor which takes the QP dimension and Hessian type
+ *  information. If the Hessian is the zero (i.e. HST_ZERO) or the
+ *  identity matrix (i.e. HST_IDENTITY), respectively, no memory
+ *  is allocated for it and a NULL pointer can be passed for it
+ *  to the init() functions. */
+void QProblemBCON(	QProblemB* _THIS,
+					int _nV,						/**< Number of variables. */
+					HessianType _hessianType		/**< Type of Hessian matrix. */
+					);
+
+void QProblemBCPY(	QProblemB* FROM,
+					QProblemB* TO
+					);
+
+
+/** Clears all data structures of QProblemB except for QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_RESET_FAILED */
+returnValue QProblemB_reset( QProblemB* _THIS );
+
+
+/** Initialises a simply bounded QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_initM(	QProblemB* _THIS,
+								DenseMatrix *_H,	 		/**< Hessian matrix. */
+								const real_t* const _g,		/**< Gradient vector. */
+								const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ub,	/**< Upper bounds (on variables). \n
+																 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																 Output: Number of performed working set recalculations. */
+		 						real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																 Output: CPU time spent for QP initialisation (if pointer passed). */
+								);
+
+/** Initialises a simply bounded QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_init(	QProblemB* _THIS,
+							real_t* const _H, 			/**< Hessian matrix. \n
+															 If Hessian matrix is trivial, a NULL pointer can be passed. */
+							const real_t* const _g,		/**< Gradient vector. */
+							const real_t* const _lb,	/**< Lower bounds (on variables). \n
+															 If no lower bounds exist, a NULL pointer can be passed. */
+							const real_t* const _ub,	/**< Upper bounds (on variables). \n
+															 If no upper bounds exist, a NULL pointer can be passed. */
+							int* nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+															 Output: Number of performed working set recalculations. */
+		 					real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+															 Output: CPU time spent for QP initialisation (if pointer passed). */
+							);
+
+/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it
+ *	using at most nWSR iterations.
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblemB_initF(	QProblemB* _THIS,
+								const char* const H_file, 	/**< Name of file where Hessian matrix is stored. \n
+																 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const char* const g_file,  	/**< Name of file where gradient vector is stored. */
+								const char* const lb_file, 	/**< Name of file where lower bound vector. \n
+																 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 	/**< Name of file where upper bound vector. \n
+																 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 					/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																 Output: Number of performed working set recalculations. */
+		 						real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP initialisation. \n
+																 Output: CPU time spent for QP initialisation (if pointer passed). */
+								);
+
+/** Initialises a simply bounded QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n
+ *	3. 0,    yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n
+ *	4. 0,    0,    gB: starts with xOpt = 0, yOpt = 0 and gB, \n
+ *	5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n
+ *	6. xOpt, 0,    gB: starts with xOpt, yOpt = 0 and gB, \n
+ *	7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_initMW(	QProblemB* _THIS,
+								DenseMatrix *_H,				/**< Hessian matrix. */
+								const real_t* const _g,			/**< Gradient vector. */
+								const real_t* const _lb,		/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ub,		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 						/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+		 						real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP initialisation. \n
+													 				 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,		/**< Optimal primal solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,		/**< Optimal dual solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,	/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																	 (If a null pointer is passed, all bounds are assumed inactive!) */
+								const real_t* const _R			/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																	 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																	 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Initialises a simply bounded QP problem with given QP data and tries to solve it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n
+ *	3. 0,    yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n
+ *	4. 0,    0,    gB: starts with xOpt = 0, yOpt = 0 and gB, \n
+ *	5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n
+ *	6. xOpt, 0,    gB: starts with xOpt, yOpt = 0 and gB, \n
+ *	7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_initW(	QProblemB* _THIS,
+								real_t* const _H, 				/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const real_t* const _g,			/**< Gradient vector. */
+								const real_t* const _lb,		/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const _ub,		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 						/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+		 						real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP initialisation. \n
+													 				 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,		/**< Optimal primal solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,		/**< Optimal dual solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,	/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																	 (If a null pointer is passed, all bounds are assumed inactive!) */
+								const real_t* const _R			/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix.
+																 	 The Cholesky factor must be stored in a real_t array of size nV*nV
+																	 in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n
+																	 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it
+ *	using at most nWSR iterations. Depending on the parameter constellation it: \n
+ *	1. 0,    0,    0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n
+ *	2. xOpt, 0,    0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n
+ *	3. 0,    yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n
+ *	4. 0,    0,    gB: starts with xOpt = 0, yOpt = 0 and gB, \n
+ *	5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n
+ *	6. xOpt, 0,    gB: starts with xOpt, yOpt = 0 and gB, \n
+ *	7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!)
+ *
+ *  Note: This function internally calls solveInitialQP for initialisation!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED \n
+			RET_UNABLE_TO_READ_FILE */
+returnValue QProblemB_initFW(	QProblemB* _THIS,
+								const char* const H_file, 		/**< Name of file where Hessian matrix is stored. \n
+																	 If Hessian matrix is trivial, a NULL pointer can be passed. */
+								const char* const g_file,  		/**< Name of file where gradient vector is stored. */
+								const char* const lb_file, 		/**< Name of file where lower bound vector. \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+								const char* const ub_file, 		/**< Name of file where upper bound vector. \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR, 						/**< Input: Maximum number of working set recalculations when using initial homotopy. \n
+																	 Output: Number of performed working set recalculations. */
+		 						real_t* const cputime,			/**< Input: Maximum CPU time allowed for QP initialisation. \n
+													 				 Output: CPU time spent for QP initialisation. */
+								const real_t* const xOpt,		/**< Optimal primal solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old primal solution is kept!) */
+								const real_t* const yOpt,		/**< Optimal dual solution vector. A NULL pointer can be passed. \n
+																	 (If a null pointer is passed, the old dual solution is kept!) */
+								Bounds* const guessedBounds,	/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																	 (If a null pointer is passed, all bounds are assumed inactive!) */
+								const char* const R_file		/**< Name of the file where a pre-computed (upper triangular) Cholesky factor
+																	 of the Hessian matrix is stored. \n
+																	 (If a null pointer is passed, Cholesky decomposition is computed internally!) */
+								);
+
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	By default, QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_MAX_NWSR_REACHED \n
+			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblemB_hotstart(	QProblemB* _THIS,
+								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+											 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+											 					 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime 		/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *  where QP data is read from files. QP solution is started from previous solution.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_hotstartF(	QProblemB* _THIS,
+									const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+									const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 						 If no lower bounds exist, a NULL pointer can be passed. */
+									const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 						 If no upper bounds exist, a NULL pointer can be passed. */
+									int* nWSR, 					/**< Input: Maximum number of working set recalculations; \n
+																	 Output: Number of performed working set recalculations. */
+									real_t* const cputime 	 	/**< Input: Maximum CPU time allowed for QP solution. \n
+																	 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+									);
+
+/** Solves an initialised QP sequence using the online active set strategy.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+			RET_MAX_NWSR_REACHED \n
+			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_SETUP_AUXILIARYQP_FAILED */
+returnValue QProblemB_hotstartW(	QProblemB* _THIS,
+									const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+									const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+											 						 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+											 						 If no upper bounds exist, a NULL pointer can be passed. */
+									int* nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																	 Output: Number of performed working set recalculations. */
+									real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+									Bounds* const guessedBounds	/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																	 (If a null pointer is passed, the previous working set is kept!) */
+									);
+
+/** Solves an initialised QP sequence using the online active set strategy,
+ *  where QP data is read from files.
+ *	By default, QP solution is started from previous solution. If a guess
+ *	for the working set is provided, an initialised homotopy is performed.
+ *
+ *  Note: This function internally calls solveQP/solveRegularisedQP
+ *        for solving an initialised QP!
+ *
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_MAX_NWSR_REACHED \n
+ 			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_SETUP_AUXILIARYQP_FAILED */
+returnValue QProblemB_hotstartFW(	QProblemB* _THIS,
+									const char* const g_file,	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+									const char* const lb_file,	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+											 						 If no lower bounds exist, a NULL pointer can be passed. */
+									const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+											 						 If no upper bounds exist, a NULL pointer can be passed. */
+									int* nWSR,					/**< Input:  Maximum number of working set recalculations; \n
+																	 Output: Number of performed working set recalculations. */
+									real_t* const cputime,		/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+									Bounds* const guessedBounds	/**< Optimal working set of bounds for solution (xOpt,yOpt). \n
+																	 (If a null pointer is passed, the previous working set is kept!) */
+									);
+
+
+/** Writes a vector with the state of the working set
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblemB_getWorkingSet(	QProblemB* _THIS,
+										real_t* workingSet		/** Output: array containing state of the working set. */
+										);
+
+/** Writes a vector with the state of the working set of bounds
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblemB_getWorkingSetBounds(	QProblemB* _THIS,
+											real_t* workingSetB	/** Output: array containing state of the working set of bounds. */
+											);
+
+/** Writes a vector with the state of the working set of constraints
+ *	\return SUCCESSFUL_RETURN \n
+ *	        RET_INVALID_ARGUMENTS */
+returnValue QProblemB_getWorkingSetConstraints(	QProblemB* _THIS,
+												real_t* workingSetC	/** Output: array containing state of the working set of constraints. */
+												);
+
+
+/** Returns current bounds object of the QP (deep copy).
+  *	\return SUCCESSFUL_RETURN \n
+  			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblemB_getBounds(	QProblemB* _THIS,
+												Bounds* _bounds	/** Output: Bounds object. */
+												);
+
+
+/** Returns the number of variables.
+ *	\return Number of variables. */
+static inline int QProblemB_getNV( QProblemB* _THIS );
+
+/** Returns the number of free variables.
+ *	\return Number of free variables. */
+static inline int QProblemB_getNFR( QProblemB* _THIS );
+
+/** Returns the number of fixed variables.
+ *	\return Number of fixed variables. */
+static inline int QProblemB_getNFX( QProblemB* _THIS );
+
+/** Returns the number of implicitly fixed variables.
+ *	\return Number of implicitly fixed variables. */
+static inline int QProblemB_getNFV( QProblemB* _THIS );
+
+/** Returns the dimension of null space.
+ *	\return Dimension of null space. */
+int QProblemB_getNZ( QProblemB* _THIS );
+
+
+/** Returns the optimal objective function value.
+ *	\return finite value: Optimal objective function value (QP was solved) \n
+ 			+infinity:	  QP was not yet solved */
+real_t QProblemB_getObjVal( QProblemB* _THIS );
+
+/** Returns the objective function value at an arbitrary point x.
+ *	\return Objective function value at point x */
+real_t QProblemB_getObjValX(	QProblemB* _THIS,
+								const real_t* const _x	/**< Point at which the objective function shall be evaluated. */
+								);
+
+/** Returns the primal solution vector.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblemB_getPrimalSolution(	QProblemB* _THIS,
+											real_t* const xOpt			/**< Output: Primal solution vector (if QP has been solved). */
+											);
+
+/** Returns the dual solution vector.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_QP_NOT_SOLVED */
+returnValue QProblemB_getDualSolution(	QProblemB* _THIS,
+										real_t* const yOpt	/**< Output: Dual solution vector (if QP has been solved). */
+										);
+
+
+/** Returns status of the solution process.
+ *	\return Status of solution process. */
+static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS );
+
+
+/** Returns if the QProblem object is initialised.
+ *	\return BT_TRUE:  QProblemB initialised \n
+ 			BT_FALSE: QProblemB not initialised */
+static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS );
+
+/** Returns if the QP has been solved.
+ *	\return BT_TRUE:  QProblemB solved \n
+ 			BT_FALSE: QProblemB not solved */
+static inline BooleanType QProblemB_isSolved( QProblemB* _THIS );
+
+/** Returns if the QP is infeasible.
+ *	\return BT_TRUE:  QP infeasible \n
+ 			BT_FALSE: QP feasible (or not known to be infeasible!) */
+static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS );
+
+/** Returns if the QP is unbounded.
+ *	\return BT_TRUE:  QP unbounded \n
+ 			BT_FALSE: QP unbounded (or not known to be unbounded!) */
+static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS );
+
+
+/** Returns Hessian type flag (type is not determined due to _THIS call!).
+ *	\return Hessian type. */
+static inline HessianType QProblemB_getHessianType( QProblemB* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblemB_setHessianType(	QProblemB* _THIS,
+													HessianType _hessianType /**< New Hessian type. */
+													);
+
+/** Returns if the QP has been internally regularised.
+ *	\return BT_TRUE:  Hessian is internally regularised for QP solution \n
+ 			BT_FALSE: No internal Hessian regularisation is used for QP solution */
+static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS );
+
+/** Returns current options struct.
+ *	\return Current options struct. */
+static inline Options QProblemB_getOptions( QProblemB* _THIS );
+
+/** Overrides current options with given ones.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblemB_setOptions(	QProblemB* _THIS,
+												Options _options	/**< New options. */
+												);
+
+/** Returns the print level.
+ *	\return Print level. */
+static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS );
+
+/** Changes the print level.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemB_setPrintLevel(	QProblemB* _THIS,
+										PrintLevel _printlevel	/**< New print level. */
+										);
+
+/** Returns the current number of QP problems solved.
+ *	\return Number of QP problems solved. */
+static inline unsigned int QProblemB_getCount( QProblemB* _THIS );
+
+/** Resets QP problem counter (to zero).
+ *	\return SUCCESSFUL_RETURN. */
+static inline returnValue QProblemB_resetCounter( QProblemB* _THIS );
+
+
+/** Prints concise list of properties of the current QP.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblemB_printProperties( QProblemB* _THIS );
+
+/** Prints a list of all options and their current values.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblemB_printOptions( QProblemB* _THIS );
+
+
+/** If Hessian type has been set by the user, nothing is done.
+ *  Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or
+ *  HST_POSDEF (default), respectively.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_INDEFINITE */
+returnValue QProblemB_determineHessianType( QProblemB* _THIS );
+
+/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblemB_setupSubjectToType( QProblemB* _THIS );
+
+/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUPSUBJECTTOTYPE_FAILED */
+returnValue QProblemB_setupSubjectToTypeNew(	QProblemB* _THIS,
+												const real_t* const lb_new,	/**< New lower bounds. */
+												const real_t* const ub_new	/**< New upper bounds. */
+												);
+
+/** Computes the Cholesky decomposition of the (simply projected) Hessian
+ *  (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple
+ *  projection matrix!
+ *  Note: If Hessian turns out not to be positive definite, the Hessian type
+ *		  is set to HST_SEMIDEF accordingly.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblemB_computeCholesky( QProblemB* _THIS );
+
+/** Computes initial Cholesky decomposition of the projected Hessian making
+ *  use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected().
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_HESSIAN_NOT_SPD \n
+ *			RET_INDEXLIST_CORRUPTED */
+returnValue QProblemB_setupInitialCholesky( QProblemB* _THIS );
+
+
+/** Obtains the desired working set for the auxiliary initial QP in
+ *  accordance with the user specifications
+ *	\return SUCCESSFUL_RETURN \n
+			RET_OBTAINING_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_obtainAuxiliaryWorkingSet(	QProblemB* _THIS,
+													const real_t* const xOpt,		/**< Optimal primal solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													const real_t* const yOpt,		/**< Optimal dual solution vector.
+																					 *	 If a NULL pointer is passed, all entries are assumed to be zero. */
+													Bounds* const guessedBounds,	/**< Guessed working set for solution (xOpt,yOpt). */
+													Bounds* auxiliaryBounds			/**< Input: Allocated bound object. \n
+																					 *	 Ouput: Working set for auxiliary QP. */
+													);
+
+/** Decides if lower bounds are smaller than upper bounds
+ *
+ * \return SUCCESSFUL_RETURN \n
+ * 		   RET_QP_INFEASIBLE */
+returnValue QProblemB_areBoundsConsistent(	QProblemB* _THIS,
+											const real_t* const lb, /**< Vector of lower bounds*/
+											const real_t* const ub  /**< Vector of upper bounds*/
+											);
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblemB_backsolveR(	QProblemB* _THIS,
+									const real_t* const b,	/**< Right hand side vector. */
+									BooleanType transposed,	/**< Indicates if the transposed system shall be solved. */
+									real_t* const a 		/**< Output: Solution vector */
+									);
+
+/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n
+ *  Special variant for the case that _THIS function is called from within "removeBound()".
+ *	\return SUCCESSFUL_RETURN \n
+			RET_DIV_BY_ZERO */
+returnValue QProblemB_backsolveRrem(	QProblemB* _THIS,
+										const real_t* const b,		/**< Right hand side vector. */
+										BooleanType transposed,		/**< Indicates if the transposed system shall be solved. */
+										BooleanType removingBound,	/**< Indicates if function is called from "removeBound()". */
+										real_t* const a 			/**< Output: Solution vector */
+										);
+
+
+/** Determines step direction of the shift of the QP data.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemB_determineDataShift(	QProblemB* _THIS,
+											const real_t* const g_new,	/**< New gradient vector. */
+											const real_t* const lb_new,	/**< New lower bounds. */
+											const real_t* const ub_new,	/**< New upper bounds. */
+											real_t* const delta_g,	 	/**< Output: Step direction of gradient vector. */
+											real_t* const delta_lb,	 	/**< Output: Step direction of lower bounds. */
+											real_t* const delta_ub,	 	/**< Output: Step direction of upper bounds. */
+											BooleanType* Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */
+											);
+
+
+/** Sets up internal QP data.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_setupQPdataM(	QProblemB* _THIS,
+									DenseMatrix *_H,	 		/**< Hessian matrix.*/
+									const real_t* const _g,		/**< Gradient vector. */
+									const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									);
+
+/** Sets up internal QP data. If the current Hessian is trivial
+ *  (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemB_setupQPdata(	QProblemB* _THIS,
+									real_t* const _H, 			/**< Hessian matrix. \n
+																	 If Hessian matrix is trivial,a NULL pointer can be passed. */
+									const real_t* const _g,		/**< Gradient vector. */
+									const real_t* const _lb,	/**< Lower bounds (on variables). \n
+																	 If no lower bounds exist, a NULL pointer can be passed. */
+									const real_t* const _ub		/**< Upper bounds (on variables). \n
+																	 If no upper bounds exist, a NULL pointer can be passed. */
+									);
+
+/** Sets up internal QP data by loading it from files. If the current Hessian
+ *  is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given,
+ *  memory for Hessian is allocated and it is set to the given one.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS \n
+			RET_NO_HESSIAN_SPECIFIED */
+returnValue QProblemB_setupQPdataFromFile(	QProblemB* _THIS,
+											const char* const H_file, 	/**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n
+														     				 If Hessian matrix is trivial,a NULL pointer can be passed. */
+											const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+											const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const char* const ub_file 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 						 If no upper bounds exist, a NULL pointer can be passed. */
+											);
+
+/** Loads new QP vectors from files (internal members are not affected!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNABLE_TO_OPEN_FILE \n
+			RET_UNABLE_TO_READ_FILE \n
+			RET_INVALID_ARGUMENTS */
+returnValue QProblemB_loadQPvectorsFromFile(	QProblemB* _THIS,
+												const char* const g_file, 	/**< Name of file where gradient, of neighbouring QP to be solved, is stored. */
+												const char* const lb_file, 	/**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no lower bounds exist, a NULL pointer can be passed. */
+												const char* const ub_file, 	/**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n
+										 			 							 If no upper bounds exist, a NULL pointer can be passed. */
+												real_t* const g_new,		/**< Output: Gradient of neighbouring QP to be solved. */
+												real_t* const lb_new,		/**< Output: Lower bounds of neighbouring QP to be solved */
+												real_t* const ub_new		/**< Output: Upper bounds of neighbouring QP to be solved */
+												);
+
+
+/** Sets internal infeasibility flag and throws given error in case the far bound
+ *	strategy is not enabled (as QP might actually not be infeasible in _THIS case).
+ *	\return RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_ENSURELI_FAILED_CYCLING \n
+			RET_ENSURELI_FAILED_NOINDEX */
+returnValue QProblemB_setInfeasibilityFlag(	QProblemB* _THIS,
+											returnValue returnvalue,	/**< Returnvalue to be tunneled. */
+											BooleanType doThrowError	/**< Flag forcing to throw an error. */
+											);
+
+
+/** Determines if next QP iteration can be performed within given CPU time limit.
+ *	\return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n
+			BT_FALSE: Sufficient CPU time for next QP iteration. */
+BooleanType QProblemB_isCPUtimeLimitExceeded(	QProblemB* _THIS,
+												const real_t* const cputime,	/**< Maximum CPU time allowed for QP solution. */
+												real_t starttime,				/**< Start time of current QP solution. */
+												int nWSR						/**< Number of working set recalculations performed so far. */
+												);
+
+
+/** Regularise Hessian matrix by adding a scaled identity matrix to it.
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_ALREADY_REGULARISED */
+returnValue QProblemB_regulariseHessian( QProblemB* _THIS );
+
+
+/** Sets Hessian matrix of the QP.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblemB_setHM(	QProblemB* _THIS,
+											DenseMatrix* H_new	/**< New Hessian matrix. */
+											);
+
+/** Sets dense Hessian matrix of the QP.
+ *  If a null pointer is passed and
+ *  a) hessianType is HST_IDENTITY, nothing is done,
+ *  b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero.
+ *	\return SUCCESSFUL_RETURN */
+static inline returnValue QProblemB_setH(	QProblemB* _THIS,
+											real_t* const H_new	/**< New dense Hessian matrix (with correct dimension!). */
+											);
+
+/** Changes gradient vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_INVALID_ARGUMENTS */
+static inline returnValue QProblemB_setG(	QProblemB* _THIS,
+											const real_t* const g_new	/**< New gradient vector (with correct dimension!). */
+											);
+
+/** Changes lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblemB_setLB(	QProblemB* _THIS,
+											const real_t* const lb_new	/**< New lower bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of lower bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblemB_setLBn(	QProblemB* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of lower bound vector. */
+											);
+
+/** Changes upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QPOBJECT_NOT_SETUP */
+static inline returnValue QProblemB_setUB(	QProblemB* _THIS,
+											const real_t* const ub_new	/**< New upper bound vector (with correct dimension!). */
+											);
+
+/** Changes single entry of upper bound vector of the QP.
+ *	\return SUCCESSFUL_RETURN  \n
+ *			RET_QPOBJECT_NOT_SETUP \n
+ *			RET_INDEX_OUT_OF_BOUNDS */
+static inline returnValue QProblemB_setUBn(	QProblemB* _THIS,
+											int number,		/**< Number of entry to be changed. */
+											real_t value	/**< New value for entry of upper bound vector. */
+											);
+
+
+/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0]
+ *	\return SUCCESSFUL_RETURN */
+static inline void QProblemB_computeGivens(	real_t xold,	/**< Matrix entry to be normalised. */
+											real_t yold,	/**< Matrix entry to be annihilated. */
+											real_t* xnew,	/**< Output: Normalised matrix entry. */
+											real_t* ynew,	/**< Output: Annihilated matrix entry. */
+											real_t* c,		/**< Output: Cosine entry of Givens matrix. */
+											real_t* s 		/**< Output: Sine entry of Givens matrix. */
+											);
+
+/** Applies Givens matrix determined by c and s (cf. computeGivens).
+ *	\return SUCCESSFUL_RETURN */
+static inline void QProblemB_applyGivens(	real_t c,		/**< Cosine entry of Givens matrix. */
+											real_t s,		/**< Sine entry of Givens matrix. */
+											real_t nu, 		/**< Further factor: s/(1+c). */
+											real_t xold,	/**< Matrix entry to be transformed corresponding to
+															 *	 the normalised entry of the original matrix. */
+											real_t yold, 	/**< Matrix entry to be transformed corresponding to
+															 *	 the annihilated entry of the original matrix. */
+											real_t* xnew,	/**< Output: Transformed matrix entry corresponding to
+															 *	 the normalised entry of the original matrix. */
+											real_t* ynew	/**< Output: Transformed matrix entry corresponding to
+															 *	 the annihilated entry of the original matrix. */
+											);
+
+
+
+/** Compute relative length of homotopy in data space for termination
+ *  criterion.
+ *  \return Relative length in data space. */
+real_t QProblemB_getRelativeHomotopyLength(	QProblemB* _THIS,
+											const real_t* const g_new,	/**< Final gradient. */
+											const real_t* const lb_new,	/**< Final lower variable bounds. */
+											const real_t* const ub_new	/**< Final upper variable bounds. */
+											);
+
+/** Ramping Strategy to avoid ties. Modifies homotopy start without
+ *  changing current active set.
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblemB_performRamping( QProblemB* _THIS );
+
+
+/** ... */
+returnValue QProblemB_updateFarBounds(	QProblemB* _THIS,
+										real_t curFarBound,				/**< ... */
+										int nRamp,						/**< ... */
+										const real_t* const lb_new,		/**< ... */
+										real_t* const lb_new_far,		/**< ... */
+										const real_t* const ub_new,		/**< ... */
+										real_t* const ub_new_far		/**< ... */
+										);
+
+
+
+/** Performs robustified ratio test yield the maximum possible step length
+ *  along the homotopy path.
+ *	\return  SUCCESSFUL_RETURN */
+returnValue QProblemB_performRatioTestB(	QProblemB* _THIS,
+											int nIdx, 					/**< Number of ratios to be checked. */
+											const int* const idxList, 	/**< Array containing the indices of all ratios to be checked. */
+											Bounds* const subjectTo,	/**< Bound object corresponding to ratios to be checked. */
+											const real_t* const num,	/**< Array containing all numerators for performing the ratio test. */
+											const real_t* const den,	/**< Array containing all denominators for performing the ratio test. */
+											real_t epsNum,				/**< Numerator tolerance. */
+											real_t epsDen,				/**< Denominator tolerance. */
+											real_t* t,					/**< Output: Maximum possible step length along the homotopy path. */
+											int* BC_idx 				/**< Output: Index of blocking constraint. */
+											);
+
+/** Checks whether given ratio is blocking, i.e. limits the maximum step length
+ *  along the homotopy path to a value lower than given one.
+ *	\return  SUCCESSFUL_RETURN */
+static inline BooleanType QProblemB_isBlocking(	QProblemB* _THIS,
+												real_t num,		/**< Numerator for performing the ratio test. */
+												real_t den,		/**< Denominator for performing the ratio test. */
+												real_t epsNum,	/**< Numerator tolerance. */
+												real_t epsDen,	/**< Denominator tolerance. */
+												real_t* t		/**< Input:  Current maximum step length along the homotopy path,
+																 *   Output: Updated maximum possible step length along the homotopy path. */
+												);
+
+
+/** Solves a QProblemB whose QP data is assumed to be stored in the member variables.
+ *  A guess for its primal/dual optimal solution vectors and the corresponding
+ *  optimal working set can be provided.
+ *  Note: This function is internally called by all init functions!
+ *	\return SUCCESSFUL_RETURN \n
+			RET_INIT_FAILED \n
+			RET_INIT_FAILED_CHOLESKY \n
+			RET_INIT_FAILED_HOTSTART \n
+			RET_INIT_FAILED_INFEASIBILITY \n
+			RET_INIT_FAILED_UNBOUNDEDNESS \n
+			RET_MAX_NWSR_REACHED */
+returnValue QProblemB_solveInitialQP(	QProblemB* _THIS,
+										const real_t* const xOpt,		/**< Optimal primal solution vector.*/
+										const real_t* const yOpt,		/**< Optimal dual solution vector. */
+										Bounds* const guessedBounds,	/**< Optimal working set of bounds for solution (xOpt,yOpt). */
+										const real_t* const _R,			/**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */
+										int* nWSR, 						/**< Input:  Maximum number of working set recalculations; \n
+														 				 *	 Output: Number of performed working set recalculations. */
+										real_t* const cputime			/**< Input:  Maximum CPU time allowed for QP solution. \n
+														 				 *	 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+										);
+
+/** Solves an initialised QProblemB using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+			RET_MAX_NWSR_REACHED \n
+			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblemB_solveQP(	QProblemB* _THIS,
+								const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+								const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+											 					 If no lower bounds exist, a NULL pointer can be passed. */
+								const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+											 					 If no upper bounds exist, a NULL pointer can be passed. */
+								int* nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																 Output: Number of performed working set recalculations. */
+								real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
+																 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+								int  nWSRperformed,			/**< Number of working set recalculations already performed to solve
+																 this QP within previous solveQP() calls. This number is
+																 always zero, except for successive calls from solveRegularisedQP()
+																 or when using the far bound strategy. */
+								BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
+								);
+
+
+/** Solves an initialised QProblemB using online active set strategy.
+ *  Note: This function is internally called by all hotstart functions!
+ *	\return SUCCESSFUL_RETURN \n
+			RET_MAX_NWSR_REACHED \n
+			RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n
+			RET_HOTSTART_FAILED \n
+			RET_SHIFT_DETERMINATION_FAILED \n
+			RET_STEPDIRECTION_DETERMINATION_FAILED \n
+			RET_STEPLENGTH_DETERMINATION_FAILED \n
+			RET_HOMOTOPY_STEP_FAILED \n
+			RET_HOTSTART_STOPPED_INFEASIBILITY \n
+			RET_HOTSTART_STOPPED_UNBOUNDEDNESS */
+returnValue QProblemB_solveRegularisedQP(	QProblemB* _THIS,
+											const real_t* const g_new,	/**< Gradient of neighbouring QP to be solved. */
+											const real_t* const lb_new,	/**< Lower bounds of neighbouring QP to be solved. \n
+													 						 If no lower bounds exist, a NULL pointer can be passed. */
+											const real_t* const ub_new,	/**< Upper bounds of neighbouring QP to be solved. \n
+													 						 If no upper bounds exist, a NULL pointer can be passed. */
+											int* nWSR,					/**< Input: Maximum number of working set recalculations; \n
+																			 Output: Number of performed working set recalculations. */
+											real_t* const cputime,		/**< Input: Maximum CPU time allowed for QP solution. \n
+																			 Output: CPU time spent for QP solution (or to perform nWSR iterations). */
+											int  nWSRperformed,			/**< Number of working set recalculations already performed to solve
+																			 this QP within previous solveRegularisedQP() calls. This number is
+																			 always zero, except for successive calls when using the far bound strategy. */
+											BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
+											);
+
+
+/** Sets up bound data structure according to auxiliaryBounds.
+ *  (If the working set shall be setup afresh, make sure that
+ *  bounds data structure has been resetted!)
+ *	\return SUCCESSFUL_RETURN \n
+			RET_SETUP_WORKINGSET_FAILED \n
+			RET_INVALID_ARGUMENTS \n
+			RET_UNKNOWN_BUG */
+returnValue QProblemB_setupAuxiliaryWorkingSet(	QProblemB* _THIS,
+												Bounds* const auxiliaryBounds,	/**< Working set for auxiliary QP. */
+												BooleanType setupAfresh			/**< Flag indicating if given working set shall be
+																				 *    setup afresh or by updating the current one. */
+												);
+
+/** Sets up the optimal primal/dual solution of the auxiliary initial QP.
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemB_setupAuxiliaryQPsolution(	QProblemB* _THIS,
+												const real_t* const xOpt,	/**< Optimal primal solution vector.
+																		 	 *	 If a NULL pointer is passed, all entries are set to zero. */
+												const real_t* const yOpt	/**< Optimal dual solution vector.
+																			 *	 If a NULL pointer is passed, all entries are set to zero. */
+												);
+
+/** Sets up gradient of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS have already been (ialised!).
+ *	\return SUCCESSFUL_RETURN */
+returnValue QProblemB_setupAuxiliaryQPgradient( QProblemB* _THIS );
+
+/** Sets up bounds of the auxiliary initial QP for given
+ *  optimal primal/dual solution and given initial working set
+ *  (assumes that members X, Y and BOUNDS have already been initialised!).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_UNKNOWN_BUG */
+returnValue QProblemB_setupAuxiliaryQPbounds(	QProblemB* _THIS,
+												BooleanType useRelaxation	/**< Flag indicating if inactive bounds shall be relaxed. */
+												);
+
+
+/** Updates QP vectors, working sets and internal data structures in order to
+	start from an optimal solution corresponding to initial guesses of the working
+	set for bounds
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_SETUP_AUXILIARYQP_FAILED */
+returnValue QProblemB_setupAuxiliaryQP(	QProblemB* _THIS,
+										Bounds* const guessedBounds	/**< Initial guess for working set of bounds. */
+										);
+
+/** Determines step direction of the homotopy path.
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_STEPDIRECTION_FAILED_CHOLESKY */
+returnValue QProblemB_determineStepDirection(	QProblemB* _THIS,
+												const real_t* const delta_g,	/**< Step direction of gradient vector. */
+												const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+												const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+												BooleanType Delta_bB_isZero,	/**< Indicates if active bounds are to be shifted. */
+												real_t* const delta_xFX, 		/**< Output: Primal homotopy step direction of fixed variables. */
+												real_t* const delta_xFR,	 	/**< Output: Primal homotopy step direction of free variables. */
+												real_t* const delta_yFX 		/**< Output: Dual homotopy step direction of fixed variables' multiplier. */
+												);
+
+/** Determines the maximum possible step length along the homotopy path
+ *  and performs _THIS step (without changing working set).
+ *	\return SUCCESSFUL_RETURN \n
+ *			RET_QP_INFEASIBLE \n
+ */
+returnValue QProblemB_performStep(	QProblemB* _THIS,
+									const real_t* const delta_g,	/**< Step direction of gradient. */
+									const real_t* const delta_lb,	/**< Step direction of lower bounds. */
+									const real_t* const delta_ub,	/**< Step direction of upper bounds. */
+									const real_t* const delta_xFX, 	/**< Primal homotopy step direction of fixed variables. */
+									const real_t* const delta_xFR,	/**< Primal homotopy step direction of free variables. */
+									const real_t* const delta_yFX,	/**< Dual homotopy step direction of fixed variables' multiplier. */
+									int* BC_idx, 					/**< Output: Index of blocking constraint. */
+									SubjectToStatus* BC_status		/**< Output: Status of blocking constraint. */
+									);
+
+/** Updates active set.
+ *	\return  SUCCESSFUL_RETURN \n
+ 			 RET_REMOVE_FROM_ACTIVESET_FAILED \n
+			 RET_ADD_TO_ACTIVESET_FAILED */
+returnValue QProblemB_changeActiveSet(	QProblemB* _THIS,
+										int BC_idx, 				/**< Index of blocking constraint. */
+										SubjectToStatus BC_status 	/**< Status of blocking constraint. */
+										);
+
+/** Drift correction at end of each active set iteration
+ *  \return SUCCESSFUL_RETURN */
+returnValue QProblemB_performDriftCorrection( QProblemB* _THIS );
+
+/** Determines if it is more efficient to refactorise the matrices when
+ *  hotstarting or not (i.e. better to update the existing factorisations).
+ *	\return BT_TRUE iff matrices shall be refactorised afresh
+ */
+BooleanType QProblemB_shallRefactorise(	QProblemB* _THIS,
+										Bounds* const guessedBounds	/**< Guessed new working set. */
+										);
+
+
+/** Adds a bound to active set (specialised version for the case where no constraints exist).
+ *	\return SUCCESSFUL_RETURN \n
+ 			RET_ADDBOUND_FAILED */
+returnValue QProblemB_addBound(	QProblemB* _THIS,
+								int number,					/**< Number of bound to be added to active set. */
+								SubjectToStatus B_status,	/**< Status of new active bound. */
+								BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
+								);
+
+/** Removes a bounds from active set (specialised version for the case where no constraints exist).
+ *	\return SUCCESSFUL_RETURN \n
+			RET_HESSIAN_NOT_SPD \n
+			RET_REMOVEBOUND_FAILED */
+returnValue QProblemB_removeBound(	QProblemB* _THIS,
+									int number,					/**< Number of bound to be removed from active set. */
+									BooleanType updateCholesky	/**< Flag indicating if Cholesky decomposition shall be updated. */
+									);
+
+
+/** Prints concise information on the current iteration.
+ *	\return  SUCCESSFUL_RETURN \n */
+returnValue QProblemB_printIteration(	QProblemB* _THIS,
+										int iter,					/**< Number of current iteration. */
+										int BC_idx, 				/**< Index of blocking bound. */
+										SubjectToStatus BC_status,	/**< Status of blocking bound. */
+										real_t homotopyLength,		/**< Current homotopy distance. */
+										BooleanType isFirstCall		/**< Indicating whether this is the first call for current QP. */
+										);
+
+
+
+/*
+ *	g e t B o u n d s
+ */
+static inline returnValue QProblemB_getBounds( QProblemB* _THIS, Bounds* _bounds )
+{
+	int nV = QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	_bounds = _THIS->bounds;
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t N V
+ */
+static inline int QProblemB_getNV( QProblemB* _THIS )
+{
+	return Bounds_getNV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F R
+ */
+static inline int QProblemB_getNFR( QProblemB* _THIS )
+{
+	return Bounds_getNFR( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F X
+ */
+static inline int QProblemB_getNFX( QProblemB* _THIS )
+{
+	return Bounds_getNFX( _THIS->bounds );
+}
+
+
+/*
+ *	g e t N F V
+ */
+static inline int QProblemB_getNFV( QProblemB* _THIS )
+{
+	return Bounds_getNFV( _THIS->bounds );
+}
+
+
+/*
+ *	g e t S t a t u s
+ */
+static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS )
+{
+	return _THIS->status;
+}
+
+
+/*
+ *	i s I n i t i a l i s e d
+ */
+static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS )
+{
+	if ( _THIS->status == QPS_NOTINITIALISED )
+		return BT_FALSE;
+	else
+		return BT_TRUE;
+}
+
+
+/*
+ *	i s S o l v e d
+ */
+static inline BooleanType QProblemB_isSolved( QProblemB* _THIS )
+{
+	if ( _THIS->status == QPS_SOLVED )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	i s I n f e a s i b l e
+ */
+static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS )
+{
+	return _THIS->infeasible;
+}
+
+
+/*
+ *	i s U n b o u n d e d
+ */
+static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS )
+{
+	return _THIS->unbounded;
+}
+
+
+/*
+ *	g e t H e s s i a n T y p e
+ */
+static inline HessianType QProblemB_getHessianType( QProblemB* _THIS )
+{
+	return _THIS->hessianType;
+}
+
+
+/*
+ *	s e t H e s s i a n T y p e
+ */
+static inline returnValue QProblemB_setHessianType( QProblemB* _THIS, HessianType _hessianType )
+{
+	_THIS->hessianType = _hessianType;
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	u s i n g R e g u l a r i s a t i o n
+ */
+static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS )
+{
+	if ( _THIS->regVal > QPOASES_ZERO )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *	g e t O p t i o n s
+ */
+static inline Options QProblemB_getOptions( QProblemB* _THIS )
+{
+	return _THIS->options;
+}
+
+
+/*
+ *	s e t O p t i o n s
+ */
+static inline returnValue QProblemB_setOptions(	QProblemB* _THIS,
+												Options _options
+												)
+{
+	OptionsCPY( &_options,&(_THIS->options) );
+	Options_ensureConsistency( &(_THIS->options) );
+
+	QProblemB_setPrintLevel( _THIS,_THIS->options.printLevel );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	g e t P r i n t L e v e l
+ */
+static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS )
+{
+	return _THIS->options.printLevel;
+}
+
+
+
+/*
+ *	g e t C o u n t
+ */
+static inline unsigned int QProblemB_getCount( QProblemB* _THIS )
+{
+	return _THIS->count;
+}
+
+
+/*
+ *	r e s e t C o u n t e r
+ */
+static inline returnValue QProblemB_resetCounter( QProblemB* _THIS )
+{
+	_THIS->count = 0;
+	return SUCCESSFUL_RETURN;
+}
+
+
+
+/*****************************************************************************
+ *  P R O T E C T E D                                                        *
+ *****************************************************************************/
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblemB_setHM( QProblemB* _THIS, DenseMatrix* H_new )
+{
+	if ( H_new == 0 )
+		return QProblemB_setH( _THIS,(real_t*)0 );
+	else
+		return QProblemB_setH( _THIS,DenseMatrix_getVal(H_new) );
+}
+
+
+/*
+ *	s e t H
+ */
+static inline returnValue QProblemB_setH( QProblemB* _THIS, real_t* const H_new )
+{
+	/* if null pointer is passed, Hessian is set to zero matrix
+	 *                            (or stays identity matrix) */
+	if ( H_new == 0 )
+	{
+		if ( _THIS->hessianType == HST_IDENTITY )
+			return SUCCESSFUL_RETURN;
+
+		_THIS->hessianType = HST_ZERO;
+
+		_THIS->H = 0;
+	}
+	else
+	{
+		DenseMatrixCON( _THIS->H,QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),H_new );
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t G
+ */
+static inline returnValue QProblemB_setG( QProblemB* _THIS, const real_t* const g_new )
+{
+	unsigned int nV = (unsigned int)QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( g_new == 0 )
+		return THROWERROR( RET_INVALID_ARGUMENTS );
+
+	memcpy( _THIS->g,g_new,nV*sizeof(real_t) );
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblemB_setLB( QProblemB* _THIS, const real_t* const lb_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( lb_new != 0 )
+	{
+		memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no lower bounds are specified, set them to -infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->lb[i] = -QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t L B
+ */
+static inline returnValue QProblemB_setLBn( QProblemB* _THIS, int number, real_t value )
+{
+	int nV = QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->lb[number] = value;
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblemB_setUB( QProblemB* _THIS, const real_t* const ub_new )
+{
+	unsigned int i;
+	unsigned int nV = (unsigned int)QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ub_new != 0 )
+	{
+		memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) );
+	}
+	else
+	{
+		/* if no upper bounds are specified, set them to infinity */
+		for( i=0; i<nV; ++i )
+			_THIS->ub[i] = QPOASES_INFTY;
+	}
+
+	return SUCCESSFUL_RETURN;
+}
+
+
+/*
+ *	s e t U B
+ */
+static inline returnValue QProblemB_setUBn( QProblemB* _THIS, int number, real_t value )
+{
+	int nV = QProblemB_getNV( _THIS );
+
+	if ( nV == 0 )
+		return THROWERROR( RET_QPOBJECT_NOT_SETUP );
+
+	if ( ( number >= 0 ) && ( number < nV ) )
+	{
+		_THIS->ub[number] = value;
+
+		return SUCCESSFUL_RETURN;
+	}
+	else
+	{
+		return THROWERROR( RET_INDEX_OUT_OF_BOUNDS );
+	}
+}
+
+
+/*
+ *	c o m p u t e G i v e n s
+ */
+static inline void QProblemB_computeGivens(	real_t xold, real_t yold,
+											real_t* xnew, real_t* ynew, real_t* c, real_t* s
+											)
+{
+	real_t t, mu;
+
+	if ( fabs( yold ) <= QPOASES_ZERO )
+	{
+		*c = 1.0;
+		*s = 0.0;
+
+		*xnew = xold;
+		*ynew = yold;
+	}
+	else
+	{
+		mu = fabs( xold );
+		if ( fabs( yold ) > mu )
+			mu = fabs( yold );
+
+		t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) );
+
+		if ( xold < 0.0 )
+		t = -t;
+
+		*c = xold/t;
+		*s = yold/t;
+		*xnew = t;
+		*ynew = 0.0;
+	}
+
+	return;
+}
+
+
+/*
+ *	a p p l y G i v e n s
+ */
+static inline void QProblemB_applyGivens(	real_t c, real_t s, real_t nu, real_t xold, real_t yold,
+											real_t* xnew, real_t* ynew
+											)
+{
+	#ifdef __USE_THREE_MULTS_GIVENS__
+
+	/* Givens plane rotation requiring only three multiplications,
+	 * cf. Hammarling, S.: A note on modifications to the givens plane rotation.
+	 * J. Inst. Maths Applics, 13:215-218, 1974. */
+	*xnew = xold*c + yold*s;
+	*ynew = (*xnew+xold)*nu - yold;
+
+	#else
+
+	/* Usual Givens plane rotation requiring four multiplications. */
+	*xnew =  c*xold + s*yold;
+	*ynew = -s*xold + c*yold;
+
+	#endif
+
+	return;
+}
+
+
+/*
+ * i s B l o c k i n g
+ */
+static inline BooleanType QProblemB_isBlocking(	QProblemB* _THIS,
+												real_t num,
+												real_t den,
+												real_t epsNum,
+												real_t epsDen,
+												real_t* t
+												)
+{
+	if ( ( den >= epsDen ) && ( num >= epsNum ) )
+	{
+		if ( num < (*t)*den )
+			return BT_TRUE;
+	}
+
+	return BT_FALSE;
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_QPROBLEMB_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Types.h b/third_party/acados/include/qpOASES_e/Types.h
new file mode 100644
index 000000000..1e452097f
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Types.h
@@ -0,0 +1,310 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Types.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of all non-built-in types (except for classes).
+ */
+
+
+#ifndef QPOASES_TYPES_H
+#define QPOASES_TYPES_H
+
+#ifdef USE_ACADOS_TYPES
+#include "acados/utils/types.h"
+#endif
+
+/* If your compiler does not support the snprintf() function,
+ * uncomment the following line and try to compile again. */
+/* #define __NO_SNPRINTF__ */
+
+
+/* Uncomment the following line for setting the __DSPACE__ flag. */
+/* #define __DSPACE__ */
+
+/* Uncomment the following line for setting the __XPCTARGET__ flag. */
+/* #define __XPCTARGET__ */
+
+
+/* Uncomment the following line for setting the __NO_FMATH__ flag. */
+/* #define __NO_FMATH__ */
+
+/* Uncomment the following line to enable debug information. */
+/* #define __DEBUG__ */
+
+/* Uncomment the following line to enable suppress any kind of console output. */
+/* #define __SUPPRESSANYOUTPUT__ */
+
+
+/** Forces to always include all implicitly fixed bounds and all equality constraints
+ *  into the initial working set when setting up an auxiliary QP. */
+#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__
+
+/* Uncomment the following line to activate the use of an alternative Givens
+ * plane rotation requiring only three multiplications. */
+/* #define __USE_THREE_MULTS_GIVENS__ */
+
+/* Uncomment the following line to activate the use of single precision arithmetic. */
+/* #define __USE_SINGLE_PRECISION__ */
+
+/* The inline keyword is skipped by default as it is not part of the C90 standard.
+ * However, by uncommenting the following line, use of the inline keyword can be enforced. */
+/* #define __USE_INLINE__ */
+
+
+/* Work-around for Borland BCC 5.5 compiler. */
+#ifdef __BORLANDC__
+#if __BORLANDC__ < 0x0561
+  #define __STDC__ 1
+#endif
+#endif
+
+
+/* Work-around for Microsoft compilers. */
+#ifdef _MSC_VER
+  #define __NO_SNPRINTF__
+  #pragma warning( disable : 4061 4100 4250 4514 4996 )
+#endif
+
+
+/* Apply pre-processor settings when using qpOASES within auto-generated code. */
+#ifdef __CODE_GENERATION__
+  #define __NO_COPYRIGHT__
+  #define __EXTERNAL_DIMENSIONS__
+#endif /* __CODE_GENERATION__ */
+
+
+/* Avoid using static variables declaration within functions. */
+#ifdef __NO_STATIC__
+  #define myStatic
+#else
+  #define myStatic static
+#endif /* __NO_STATIC__ */
+
+
+/* Skip inline keyword if not specified otherwise. */
+#ifndef __USE_INLINE__
+  #define inline
+#endif
+
+
+/* Avoid any printing on embedded platforms. */
+#if defined(__DSPACE__) || defined(__XPCTARGET__)
+  #define __SUPPRESSANYOUTPUT__
+  #define __NO_SNPRINTF__
+#endif
+
+
+#ifdef __NO_SNPRINTF__
+  #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__)
+    /* If snprintf is not available, provide an empty implementation... */
+    int snprintf( char* s, size_t n, const char* format, ... );
+  #else
+	/* ... or substitute snprintf by _snprintf for Microsoft compilers. */
+    #define snprintf _snprintf
+  #endif
+#endif /* __NO_SNPRINTF__ */
+
+
+/** Macro for switching on/off the beginning of the qpOASES namespace definition. */
+#define BEGIN_NAMESPACE_QPOASES
+
+/** Macro for switching on/off the end of the qpOASES namespace definition. */
+#define END_NAMESPACE_QPOASES
+
+/** Macro for switching on/off the use of the qpOASES namespace. */
+#define USING_NAMESPACE_QPOASES
+
+/** Macro for switching on/off references to the qpOASES namespace. */
+#define REFER_NAMESPACE_QPOASES /*::*/
+
+
+/** Macro for accessing the Cholesky factor R. */
+#define RR( I,J )  _THIS->R[(I)+nV*(J)]
+
+/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */
+#define QQ( I,J )  _THIS->Q[(I)+nV*(J)]
+
+/** Macro for accessing the triangular matrix T of the QT factorisation. */
+#define TT( I,J )  _THIS->T[(I)*nVC_min+(J)]
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Defines real_t for facilitating switching between double and float. */
+
+#ifndef USE_ACADOS_TYPES
+#ifndef __CODE_GENERATION__
+
+  #ifdef __USE_SINGLE_PRECISION__
+  typedef float real_t;
+  #else
+  typedef double real_t;
+  #endif /* __USE_SINGLE_PRECISION__ */
+
+#endif /* __CODE_GENERATION__ */
+#endif /* USE_ACADOS_TYPES */
+
+/** Summarises all possible logical values. */
+typedef enum
+{
+	BT_FALSE = 0,				/**< Logical value for "false". */
+	BT_TRUE						/**< Logical value for "true". */
+} BooleanType;
+
+
+/** Summarises all possible print levels. Print levels are used to describe
+ *	the desired amount of output during runtime of qpOASES. */
+typedef enum
+{
+	PL_DEBUG_ITER = -2,			/**< Full tabular debugging output. */
+	PL_TABULAR,					/**< Tabular output. */
+	PL_NONE,					/**< No output. */
+	PL_LOW,						/**< Print error messages only. */
+	PL_MEDIUM,					/**< Print error and warning messages as well as concise info messages. */
+	PL_HIGH						/**< Print all messages with full details. */
+} PrintLevel;
+
+
+/** Defines visibility status of a message. */
+typedef enum
+{
+	VS_HIDDEN,					/**< Message not visible. */
+	VS_VISIBLE					/**< Message visible. */
+} VisibilityStatus;
+
+
+/** Summarises all possible states of the (S)QProblem(B) object during the
+solution process of a QP sequence. */
+typedef enum
+{
+	QPS_NOTINITIALISED,			/**< QProblem object is freshly instantiated or reset. */
+	QPS_PREPARINGAUXILIARYQP,	/**< An auxiliary problem is currently setup, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_AUXILIARYQPSOLVED,		/**< An auxilary problem was solved, either at the very beginning
+								 *   via an initial homotopy or after changing the QP matrices. */
+	QPS_PERFORMINGHOMOTOPY,		/**< A homotopy according to the main idea of the online active
+								 *   set strategy is performed. */
+	QPS_HOMOTOPYQPSOLVED,		/**< An intermediate QP along the homotopy path was solved. */
+	QPS_SOLVED					/**< The solution of the actual QP was found. */
+} QProblemStatus;
+
+
+/** Summarises all possible types of the QP's Hessian matrix. */
+typedef enum
+{
+	HST_ZERO,				/**< Hessian is zero matrix (i.e. LP formulation). */
+	HST_IDENTITY,			/**< Hessian is identity matrix. */
+	HST_POSDEF,				/**< Hessian is (strictly) positive definite. */
+	HST_POSDEF_NULLSPACE,	/**< Hessian is positive definite on null space of active bounds/constraints. */
+	HST_SEMIDEF,			/**< Hessian is positive semi-definite. */
+	HST_INDEF,				/**< Hessian is indefinite. */
+	HST_UNKNOWN				/**< Hessian type is unknown. */
+} HessianType;
+
+
+/** Summarises all possible types of bounds and constraints. */
+typedef enum
+{
+	ST_UNBOUNDED,		/**< Bound/constraint is unbounded. */
+	ST_BOUNDED,			/**< Bound/constraint is bounded but not fixed. */
+	ST_EQUALITY,		/**< Bound/constraint is fixed (implicit equality bound/constraint). */
+	ST_DISABLED,		/**< Bound/constraint is disabled (i.e. ignored when solving QP). */
+	ST_UNKNOWN			/**< Type of bound/constraint unknown. */
+} SubjectToType;
+
+
+/** Summarises all possible states of bounds and constraints. */
+typedef enum
+{
+	ST_LOWER = -1,			/**< Bound/constraint is at its lower bound. */
+	ST_INACTIVE,			/**< Bound/constraint is inactive. */
+	ST_UPPER,				/**< Bound/constraint is at its upper bound. */
+	ST_INFEASIBLE_LOWER,	/**< (to be documented) */
+	ST_INFEASIBLE_UPPER,	/**< (to be documented) */
+	ST_UNDEFINED			/**< Status of bound/constraint undefined. */
+} SubjectToStatus;
+
+
+/**
+ *	\brief Stores internal information for tabular (debugging) output.
+ *
+ *	Struct storing internal information for tabular (debugging) output
+ *	when using the (S)QProblem(B) objects.
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2013-2015
+ */
+typedef struct
+{
+	int idxAddB;		/**< Index of bound that has been added to working set. */
+	int idxRemB;		/**< Index of bound that has been removed from working set. */
+	int idxAddC;		/**< Index of constraint that has been added to working set. */
+	int idxRemC;		/**< Index of constraint that has been removed from working set. */
+	int excAddB;		/**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */
+	int excRemB;		/**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */
+	int excAddC;		/**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */
+	int excRemC;		/**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */
+} TabularOutput;
+
+/**
+ *	\brief Struct containing the variable header for mat file.
+ *
+ *	Struct storing the header of a variable to be stored in
+ *	Matlab's binary format (using the outdated Level 4 variant
+ *  for simplictiy).
+ *
+ *  Note, this code snippet has been inspired from the document
+ *  "Matlab(R) MAT-file Format, R2013b" by MathWorks
+ *
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2013-2015
+ */
+typedef struct
+{
+	long numericFormat;		/**< Flag indicating numerical format. */
+	long nRows;				/**< Number of rows. */
+	long nCols;				/**< Number of rows. */
+	long imaginaryPart;		/**< (to be documented) */
+	long nCharName;			/**< Number of character in name. */
+} MatMatrixHeader;
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_TYPES_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/UnitTesting.h b/third_party/acados/include/qpOASES_e/UnitTesting.h
new file mode 100644
index 000000000..3fb31129a
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/UnitTesting.h
@@ -0,0 +1,79 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/UnitTesting.h
+ *	\author Hans Joachim Ferreau
+ *	\version 3.1embedded
+ *	\date 2014-2015
+ *
+ *	Definition of auxiliary functions/macros for unit testing.
+ */
+
+
+#ifndef QPOASES_UNIT_TESTING_H
+#define QPOASES_UNIT_TESTING_H
+
+
+#ifndef TEST_TOL_FACTOR
+#define TEST_TOL_FACTOR 1
+#endif
+
+
+/** Return value for tests that passed. */
+#define TEST_PASSED 0
+
+/** Return value for tests that failed. */
+#define TEST_FAILED 1
+
+/** Return value for tests that could not run due to missing external data. */
+#define TEST_DATA_NOT_FOUND 99
+
+
+/** Macro verifying that two numerical values are equal in order to pass unit test. */
+#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; }
+
+/** Macro verifying that two numerical values are close to each other in order to pass unit test. */
+#define QPOASES_TEST_FOR_NEAR( x,y )  if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; }
+
+/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */
+#define QPOASES_TEST_FOR_TOL( x,tol )  if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; }
+
+/** Macro verifying that a logical expression holds in order to pass unit test. */
+#define QPOASES_TEST_FOR_TRUE( x )  if ( (x) == 0 ) { return TEST_FAILED; }
+
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_UNIT_TESTING_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/Utils.h b/third_party/acados/include/qpOASES_e/Utils.h
new file mode 100644
index 000000000..75e45a56a
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/Utils.h
@@ -0,0 +1,500 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/Utils.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of some utilities for working with the different QProblem classes.
+ */
+
+
+#ifndef QPOASES_UTILS_H
+#define QPOASES_UTILS_H
+
+#include <qpOASES_e/MessageHandling.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+
+/** Prints a vector.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printV(	const real_t* const v,	/**< Vector to be printed. */
+							int n					/**< Length of vector. */
+							);
+
+/** Prints a permuted vector.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printPV(	const real_t* const v,		/**< Vector to be printed. */
+								int n,						/**< Length of vector. */
+								const int* const V_idx		/**< Pemutation vector. */
+								);
+
+/** Prints a named vector.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printNV(	const real_t* const v,	/**< Vector to be printed. */
+								int n,					/**< Length of vector. */
+								const char* name		/** Name of vector. */
+								);
+
+/** Prints a matrix.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printM(	const real_t* const M,	/**< Matrix to be printed. */
+							int nrow,				/**< Row number of matrix. */
+							int ncol				/**< Column number of matrix. */
+							);
+
+/** Prints a permuted matrix.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printPM(	const real_t* const M,		/**< Matrix to be printed. */
+								int nrow,					/**< Row number of matrix. */
+								int ncol	,				/**< Column number of matrix. */
+								const int* const ROW_idx,	/**< Row pemutation vector. */
+								const int* const COL_idx	/**< Column pemutation vector. */
+								);
+
+/** Prints a named matrix.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printNM(	const real_t* const M,	/**< Matrix to be printed. */
+								int nrow,				/**< Row number of matrix. */
+								int ncol,				/**< Column number of matrix. */
+								const char* name		/** Name of matrix. */
+								);
+
+/** Prints an index array.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printI(	const int* const _index,	/**< Index array to be printed. */
+							int n						/**< Length of index array. */
+							);
+
+/** Prints a named index array.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printNI(	const int* const _index,	/**< Index array to be printed. */
+								int n,						/**< Length of index array. */
+								const char* name			/**< Name of index array. */
+								);
+
+
+/** Prints a string to desired output target (useful also for MATLAB output!).
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_myPrintf(	const char* s	/**< String to be written. */
+								);
+
+
+/** Prints qpOASES copyright notice.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_printCopyrightNotice( );
+
+
+/** Reads a real_t matrix from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue qpOASES_readFromFileM(	real_t* data,				/**< Matrix to be read from file. */
+									int nrow,					/**< Row number of matrix. */
+									int ncol,					/**< Column number of matrix. */
+									const char* datafilename	/**< Data file name. */
+									);
+
+/** Reads a real_t vector from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue qpOASES_readFromFileV(	real_t* data,				/**< Vector to be read from file. */
+									int n,						/**< Length of vector. */
+									const char* datafilename	/**< Data file name. */
+									);
+
+/** Reads an integer (column) vector from file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE \n
+		   RET_UNABLE_TO_READ_FILE */
+returnValue qpOASES_readFromFileI(	int* data,					/**< Vector to be read from file. */
+									int n,						/**< Length of vector. */
+									const char* datafilename	/**< Data file name. */
+									);
+
+
+/** Writes a real_t matrix into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE  */
+returnValue qpOASES_writeIntoFileM(	const real_t* const data,	/**< Matrix to be written into file. */
+									int nrow,					/**< Row number of matrix. */
+									int ncol,					/**< Column number of matrix. */
+									const char* datafilename,	/**< Data file name. */
+									BooleanType append			/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
+									);
+
+/** Writes a real_t vector into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE  */
+returnValue qpOASES_writeIntoFileV(	const real_t* const data,	/**< Vector to be written into file. */
+									int n,						/**< Length of vector. */
+									const char* datafilename,	/**< Data file name. */
+									BooleanType append			/**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */
+									);
+
+/** Writes an integer (column) vector into a file.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_UNABLE_TO_OPEN_FILE */
+returnValue qpOASES_writeIntoFileI(	const int* const integer,	/**< Integer vector to be written into file. */
+									int n,						/**< Length of vector. */
+									const char* datafilename,	/**< Data file name. */
+									BooleanType append			/**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */
+									);
+
+/** Writes a real_t matrix/vector into a Matlab binary file.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS
+ 		   RET_UNABLE_TO_WRITE_FILE */
+returnValue qpOASES_writeIntoMatFile(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
+										const real_t* const data,	/**< Data to be written into file. */
+										int nRows,					/**< Row number of matrix. */
+										int nCols, 					/**< Column number of matrix. */
+										const char* name			/**< Matlab name of matrix/vector to be stored. */
+ 										);
+
+/** Writes in integer matrix/vector into a Matlab binary file.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS
+ 		   RET_UNABLE_TO_WRITE_FILE */
+returnValue qpOASES_writeIntoMatFileI(	FILE* const matFile,		/**< Pointer to Matlab binary file. */
+										const int* const data,		/**< Data to be written into file. */
+										int nRows,					/**< Row number of matrix. */
+										int nCols,					/**< Column number of matrix. */
+										const char* name			/**< Matlab name of matrix/vector to be stored. */
+ 										);
+
+
+/** Returns the current system time.
+ * \return current system time */
+real_t qpOASES_getCPUtime( );
+
+
+/** Returns the N-norm of a vector.
+ * \return >= 0.0: successful */
+real_t qpOASES_getNorm(	const real_t* const v,	/**< Vector. */
+						int n,					/**< Vector's dimension. */
+						int type				/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+						);
+
+/** Tests whether two real-valued arguments are (numerically) equal.
+ * \return	BT_TRUE:  arguments differ not more than TOL \n
+		 	BT_FALSE: arguments differ more than TOL */
+static inline BooleanType qpOASES_isEqual(	real_t x,	/**< First real number. */
+											real_t y,	/**< Second real number. */
+											real_t TOL	/**< Tolerance for comparison. */
+											);
+
+
+/** Tests whether a real-valued argument is (numerically) zero.
+ * \return	BT_TRUE:  argument differs from 0.0 not more than TOL \n
+		 	BT_FALSE: argument differs from 0.0 more than TOL */
+static inline BooleanType qpOASES_isZero(	real_t x,			/**< Real number. */
+											real_t TOL			/**< Tolerance for comparison. */
+											);
+
+
+/** Returns sign of a real-valued argument.
+ * \return	 1.0: argument is non-negative \n
+		 	-1.0: argument is negative */
+static inline real_t qpOASES_getSign(	real_t arg	/**< real-valued argument whose sign is to be determined. */
+										);
+
+
+/** Returns maximum of two integers.
+ * \return	Maximum of two integers */
+static inline int qpOASES_getMaxI(	int x,	/**< First integer. */
+									int y	/**< Second integer. */
+									);
+
+
+/** Returns minimum of two integers.
+ * \return	Minimum of two integers */
+static inline int qpOASES_getMinI(	int x,	/**< First integer. */
+									int y	/**< Second integer. */
+									);
+
+
+/** Returns maximum of two reals.
+ * \return	Maximum of two reals */
+static inline real_t qpOASES_getMax(	real_t x,	/**< First real number. */
+										real_t y	/**< Second real number. */
+										);
+
+
+/** Returns minimum of two reals.
+ * \return	Minimum of two reals */
+static inline real_t qpOASES_getMin(	real_t x,	/**< First real number. */
+										real_t y	/**< Second real number. */
+										);
+
+
+/** Returns the absolute value of a real_t-valued argument.
+ * \return	Absolute value of a real_t-valued argument */
+static inline real_t qpOASES_getAbs(	real_t x	/**< real_t-valued argument. */
+										);
+
+/** Returns the square-root of a real number.
+ * \return	Square-root of a real number */
+static inline real_t qpOASES_getSqrt(	real_t x	/**< Non-negative real number. */
+										);
+
+
+/** Computes the maximum violation of the KKT optimality conditions
+ *	of given iterate for given QP data. */
+returnValue qpOASES_getKktViolation(	int nV,						/**< Number of variables. */
+										int nC,						/**< Number of constraints. */
+										const real_t* const H,		/**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */
+										const real_t* const g,		/**< Gradient vector. */
+										const real_t* const A,		/**< Constraint matrix. */
+										const real_t* const lb,		/**< Lower bound vector (on variables). */
+										const real_t* const ub,		/**< Upper bound vector (on variables). */
+										const real_t* const lbA,	/**< Lower constraints' bound vector. */
+										const real_t* const ubA,	/**< Upper constraints' bound vector. */
+										const real_t* const x,		/**< Primal trial vector. */
+										const real_t* const y,		/**< Dual trial vector. */
+										real_t* const _stat,		/**< Output: maximum value of stationarity condition residual. */
+										real_t* const feas,			/**< Output: maximum value of primal feasibility violation. */
+										real_t* const cmpl			/**< Output: maximum value of complementarity residual. */
+										);
+
+/** Computes the maximum violation of the KKT optimality conditions
+ *	of given iterate for given QP data. */
+returnValue qpOASES_getKktViolationSB(	int nV,						/**< Number of variables. */
+										const real_t* const H,		/**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */
+										const real_t* const g,		/**< Gradient vector. */
+										const real_t* const lb,		/**< Lower bound vector (on variables). */
+										const real_t* const ub,		/**< Upper bound vector (on variables). */
+										const real_t* const x,		/**< Primal trial vector. */
+										const real_t* const y,		/**< Dual trial vector. */
+										real_t* const _stat,		/**< Output: maximum value of stationarity condition residual. */
+										real_t* const feas,			/**< Output: maximum value of primal feasibility violation. */
+										real_t* const cmpl			/**< Output: maximum value of complementarity residual. */
+										);
+
+
+/** Writes a value of BooleanType into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_convertBooleanTypeToString(	BooleanType value, 		/**< Value to be written. */
+												char* const string		/**< Input: String of sufficient size, \n
+																			Output: String containing value. */
+												);
+
+/** Writes a value of SubjectToStatus into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_convertSubjectToStatusToString(	SubjectToStatus value,	/**< Value to be written. */
+													char* const string		/**< Input: String of sufficient size, \n
+																				Output: String containing value. */
+													);
+
+/** Writes a value of PrintLevel into a string.
+ * \return SUCCESSFUL_RETURN */
+returnValue qpOASES_convertPrintLevelToString(	PrintLevel value, 		/**< Value to be written. */
+												char* const string		/**< Input: String of sufficient size, \n
+																			Output: String containing value. */
+												);
+
+
+/** Converts a returnValue from an QProblem(B) object into a more 
+ *	simple status flag.
+ *
+ * \return  0: QP problem solved
+ *          1: QP could not be solved within given number of iterations
+ *         -1: QP could not be solved due to an internal error
+ *         -2: QP is infeasible (and thus could not be solved)
+ *         -3: QP is unbounded (and thus could not be solved)
+ */
+int qpOASES_getSimpleStatus(	returnValue returnvalue, 	/**< ReturnValue to be analysed. */
+								BooleanType doPrintStatus	/**< Flag indicating whether simple status shall be printed to screen. */
+								);
+
+/** Normalises QP constraints.
+ * \return SUCCESSFUL_RETURN \n
+ *		   RET_INVALID_ARGUMENTS */
+returnValue qpOASES_normaliseConstraints(	int nV,			/**< Number of variables. */
+											int nC, 		/**< Number of constraints. */
+											real_t* A,		/**< Input:  Constraint matrix, \n
+																 Output: Normalised constraint matrix. */
+											real_t* lbA,	/**< Input:  Constraints' lower bound vector, \n
+																 Output: Normalised constraints' lower bound vector. */
+											real_t* ubA,	/**< Input:  Constraints' upper bound vector, \n
+																 Output: Normalised constraints' upper bound vector. */
+											int type		/**< Norm type, 1: one-norm, 2: Euclidean norm. */
+											);
+
+
+#ifdef __DEBUG__
+/** Writes matrix with given dimension into specified file. */
+void gdb_printmat(	const char *fname,			/**< File name. */
+					real_t *M,					/**< Matrix to be written. */
+					int n,						/**< Number of rows. */
+					int m,						/**< Number of columns. */
+					int ldim					/**< Leading dimension. */
+					);
+#endif /* __DEBUG__ */
+
+
+#if defined(__DSPACE__) || defined(__XPCTARGET__) 
+void __cxa_pure_virtual( void );
+#endif /* __DSPACE__ || __XPCTARGET__*/ 
+
+
+
+/*
+ *   i s E q u a l
+ */
+static inline BooleanType qpOASES_isEqual(	real_t x,
+											real_t y,
+											real_t TOL
+											)
+{
+    if ( qpOASES_getAbs(x-y) <= TOL )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *   i s Z e r o
+ */
+static inline BooleanType qpOASES_isZero(	real_t x,
+											real_t TOL
+											)
+{
+    if ( qpOASES_getAbs(x) <= TOL )
+		return BT_TRUE;
+	else
+		return BT_FALSE;
+}
+
+
+/*
+ *   g e t S i g n
+ */
+static inline real_t qpOASES_getSign(	real_t arg
+										)
+{
+	if ( arg >= 0.0 )
+		return 1.0;
+	else
+		return -1.0;
+}
+
+
+
+/*
+ *   g e t M a x
+ */
+static inline int qpOASES_getMaxI(	int x,
+									int y
+									)
+{
+    return (y<x) ? x : y;
+}
+
+
+/*
+ *   g e t M i n
+ */
+static inline int qpOASES_getMinI(	int x,
+									int y
+									)
+{
+    return (y>x) ? x : y;
+}
+
+
+/*
+ *   g e t M a x
+ */
+static inline real_t qpOASES_getMax(	real_t x,
+										real_t y
+										)
+{
+	#ifdef __NO_FMATH__
+    return (y<x) ? x : y;
+	#else
+	return (y<x) ? x : y;
+	/*return fmax(x,y); seems to be slower */
+	#endif
+}
+
+
+/*
+ *   g e t M i n
+ */
+static inline real_t qpOASES_getMin(	real_t x,
+										real_t y
+										)
+{
+	#ifdef __NO_FMATH__
+    return (y>x) ? x : y;
+	#else
+	return (y>x) ? x : y;
+	/*return fmin(x,y); seems to be slower */
+	#endif
+}
+
+
+/*
+ *   g e t A b s
+ */
+static inline real_t qpOASES_getAbs(	real_t x
+										)
+{
+	#ifdef __NO_FMATH__
+	return (x>=0.0) ? x : -x;
+	#else
+	return fabs(x);
+	#endif
+}
+
+/*
+ *   g e t S q r t
+ */
+static inline real_t qpOASES_getSqrt(	real_t x
+										)
+{
+    #ifdef __NO_FMATH__
+	return sqrt(x); /* put your custom sqrt-replacement here */
+	#else
+	return sqrt(x);
+	#endif
+}
+
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_UTILS_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/include/qpOASES_e/extras/OQPinterface.h b/third_party/acados/include/qpOASES_e/extras/OQPinterface.h
new file mode 100644
index 000000000..da59ea9db
--- /dev/null
+++ b/third_party/acados/include/qpOASES_e/extras/OQPinterface.h
@@ -0,0 +1,227 @@
+/*
+ *	This file is part of qpOASES.
+ *
+ *	qpOASES -- An Implementation of the Online Active Set Strategy.
+ *	Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka,
+ *	Christian Kirches et al. All rights reserved.
+ *
+ *	qpOASES is free software; you can redistribute it and/or
+ *	modify it under the terms of the GNU Lesser General Public
+ *	License as published by the Free Software Foundation; either
+ *	version 2.1 of the License, or (at your option) any later version.
+ *
+ *	qpOASES is distributed in the hope that it will be useful,
+ *	but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *	See the GNU Lesser General Public License for more details.
+ *
+ *	You should have received a copy of the GNU Lesser General Public
+ *	License along with qpOASES; if not, write to the Free Software
+ *	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ *
+ */
+
+
+/**
+ *	\file include/qpOASES_e/extras/OQPinterface.h
+ *	\author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches
+ *	\version 3.1embedded
+ *	\date 2007-2015
+ *
+ *	Declaration of an interface comprising several utility functions
+ *	for solving test problems from the Online QP Benchmark Collection
+ *	(This collection is no longer maintained, see
+ *	http://www.qpOASES.org/onlineQP for a backup).
+ */
+
+
+#ifndef QPOASES_OQPINTERFACE_H
+#define QPOASES_OQPINTERFACE_H
+
+
+#include <qpOASES_e/Utils.h>
+#include <qpOASES_e/Options.h>
+
+#include <qpOASES_e/QProblemB.h>
+#include <qpOASES_e/QProblem.h>
+
+
+BEGIN_NAMESPACE_QPOASES
+
+typedef struct {
+	QProblem *qp;
+
+	DenseMatrix *H;
+	DenseMatrix *A;
+
+	real_t *x;
+	real_t *y;
+} OQPbenchmark_ws;
+
+int OQPbenchmark_ws_calculateMemorySize( unsigned int nV, unsigned int nC );
+
+char *OQPbenchmark_ws_assignMemory( unsigned int nV, unsigned int nC, OQPbenchmark_ws **mem, void *raw_memory );
+
+OQPbenchmark_ws *OQPbenchmark_ws_createMemory( unsigned int nV, unsigned int nC );
+
+typedef struct {
+	QProblemB *qp;
+
+	DenseMatrix *H;
+
+	real_t *x;
+	real_t *y;
+} OQPbenchmarkB_ws;
+
+int OQPbenchmarkB_ws_calculateMemorySize( unsigned int nV );
+
+char *OQPbenchmarkB_ws_assignMemory( unsigned int nV, OQPbenchmarkB_ws **mem, void *raw_memory );
+
+OQPbenchmarkB_ws *OQPbenchmarkB_ws_createMemory( unsigned int nV );
+
+typedef struct {
+	OQPbenchmark_ws *qp_ws;
+	OQPbenchmarkB_ws *qpB_ws;
+
+	real_t *H;
+	real_t *g;
+	real_t *A;
+	real_t *lb;
+	real_t *ub;
+	real_t *lbA;
+	real_t *ubA;
+} OQPinterface_ws;
+
+int OQPinterface_ws_calculateMemorySize( unsigned int nV, unsigned int nC, unsigned int nQP  );
+
+char *OQPinterface_ws_assignMemory( unsigned int nV, unsigned int nC, unsigned int nQP, OQPinterface_ws **mem, void *raw_memory );
+
+OQPinterface_ws *OQPinterface_ws_createMemory( unsigned int nV, unsigned int nC, unsigned int nQP );
+
+/** Reads dimensions of an Online QP Benchmark problem from file.
+ * \return SUCCESSFUL_RETURN \n
+		   RET_UNABLE_TO_READ_FILE \n
+		   RET_FILEDATA_INCONSISTENT */
+returnValue readOQPdimensions(	const char* path,	/**< Full path of the data files (without trailing slash!). */
+								int* nQP,			/**< Output: Number of QPs. */
+								int* nV,			/**< Output: Number of variables. */
+								int* nC,			/**< Output: Number of constraints. */
+								int* nEC			/**< Output: Number of equality constraints. */
+								);
+
+/** Reads data of an Online QP Benchmark problem from file.
+ *  This function allocates the required memory for all data; after successfully calling it,
+ *  you have to free this memory yourself!
+ * \return SUCCESSFUL_RETURN \n
+		   RET_INVALID_ARGUMENTS \n
+		   RET_UNABLE_TO_READ_FILE \n
+		   RET_FILEDATA_INCONSISTENT */
+returnValue readOQPdata(	const char* path,	/**< Full path of the data files (without trailing slash!). */
+							int* nQP,			/**< Output: Number of QPs. */
+							int* nV,			/**< Output: Number of variables. */
+							int* nC,			/**< Output: Number of constraints. */
+							int* nEC,			/**< Output: Number of equality constraints. */
+							real_t* H,		 	/**< Output: Hessian matrix. */
+							real_t* g,		 	/**< Output: Sequence of gradient vectors. */
+							real_t* A,		 	/**< Output: Constraint matrix. */
+							real_t* lb,			/**< Output: Sequence of lower bound vectors (on variables). */
+							real_t* ub,			/**< Output: Sequence of upper bound vectors (on variables). */
+							real_t* lbA,		/**< Output: Sequence of lower constraints' bound vectors. */
+							real_t* ubA,		/**< Output: Sequence of upper constraints' bound vectors. */
+							real_t* xOpt,		/**< Output: Sequence of primal solution vectors
+												 *           (not read if a null pointer is passed). */
+							real_t* yOpt,		/**< Output: Sequence of dual solution vectors
+												 *           (not read if a null pointer is passed). */
+							real_t* objOpt		/**< Output: Sequence of optimal objective function values
+												 *           (not read if a null pointer is passed). */
+							);
+
+
+/** Solves an Online QP Benchmark problem as specified by the arguments.
+ *  The maximum deviations from the given optimal solution as well as the
+ *  maximum CPU time to solve each QP are determined.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmark(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								int nC,						/**< Number of constraints. */
+								int nEC,					/**< Number of equality constraints. */
+								real_t* _H,					/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								real_t* _A,					/**< Constraint matrix. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								const real_t* const lbA,	/**< Sequence of lower constraints' bound vectors. */
+								const real_t* const ubA,	/**< Sequence of upper constraints' bound vectors. */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options* options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t* maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t* avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t* maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t* avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t* maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t* maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t* maxComplementarity,	/**< Output: Maximum residual of complementarity condition. */
+								OQPbenchmark_ws *work	    /**< Workspace. */
+								);
+
+/** Solves an Online QP Benchmark problem (without constraints) as specified
+ *  by the arguments. The maximum deviations from the given optimal solution
+ *  as well as the maximum CPU time to solve each QP are determined.
+ * \return SUCCESSFUL_RETURN \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue solveOQPbenchmarkB(	int nQP,					/**< Number of QPs. */
+								int nV,						/**< Number of variables. */
+								real_t* _H,					/**< Hessian matrix. */
+								const real_t* const g,		/**< Sequence of gradient vectors. */
+								const real_t* const lb,		/**< Sequence of lower bound vectors (on variables). */
+								const real_t* const ub,		/**< Sequence of upper bound vectors (on variables). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options* options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t* maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t* avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t* maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t* avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t* maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t* maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t* maxComplementarity,	/**< Output: Maximum residual of complementarity condition. */
+								OQPbenchmarkB_ws *work	    /**< Workspace. */
+								);
+
+
+/** Runs an Online QP Benchmark problem and determines the maximum
+ *  violation of the KKT optimality conditions as well as the
+ *  maximum and average number of iterations and CPU time to solve
+ *	each QP.
+ *
+ * \return SUCCESSFUL_RETURN \n
+		   RET_UNABLE_TO_READ_BENCHMARK \n
+ 		   RET_BENCHMARK_ABORTED */
+returnValue runOQPbenchmark(	const char* path,			/**< Full path of the benchmark files (without trailing slash!). */
+								BooleanType isSparse,		/**< Shall convert matrices to sparse format before solution? */
+								BooleanType useHotstarts,	/**< Shall QP solution be hotstarted? */
+								const Options* options,		/**< QP solver options to be used while solving benchmark problems. */
+								int maxAllowedNWSR, 		/**< Maximum number of working set recalculations to be performed. */
+								real_t* maxNWSR,			/**< Output: Maximum number of performed working set recalculations. */
+								real_t* avgNWSR,			/**< Output: Average number of performed working set recalculations. */
+								real_t* maxCPUtime,			/**< Output: Maximum CPU time required for solving each QP. */
+								real_t* avgCPUtime,			/**< Output: Average CPU time required for solving each QP. */
+								real_t* maxStationarity,	/**< Output: Maximum residual of stationarity condition. */
+								real_t* maxFeasibility,		/**< Output: Maximum residual of primal feasibility condition. */
+								real_t* maxComplementarity,	/**< Output: Maximum residual of complementarity condition. */
+								OQPinterface_ws* work       /**< Workspace. */
+								);
+
+END_NAMESPACE_QPOASES
+
+
+#endif	/* QPOASES_OQPINTERFACE_H */
+
+
+/*
+ *	end of file
+ */
diff --git a/third_party/acados/larch64/lib/libacados.so b/third_party/acados/larch64/lib/libacados.so
new file mode 100644
index 000000000..1716e9ad6
Binary files /dev/null and b/third_party/acados/larch64/lib/libacados.so differ
diff --git a/third_party/acados/larch64/lib/libblasfeo.so b/third_party/acados/larch64/lib/libblasfeo.so
new file mode 100644
index 000000000..a48afdd8c
Binary files /dev/null and b/third_party/acados/larch64/lib/libblasfeo.so differ
diff --git a/third_party/acados/larch64/lib/libhpipm.so b/third_party/acados/larch64/lib/libhpipm.so
new file mode 100644
index 000000000..c9725f868
Binary files /dev/null and b/third_party/acados/larch64/lib/libhpipm.so differ
diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so b/third_party/acados/larch64/lib/libqpOASES_e.so
new file mode 120000
index 000000000..14048625a
--- /dev/null
+++ b/third_party/acados/larch64/lib/libqpOASES_e.so
@@ -0,0 +1 @@
+libqpOASES_e.so.3.1
\ No newline at end of file
diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1
new file mode 100644
index 000000000..0611c8a93
Binary files /dev/null and b/third_party/acados/larch64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/larch64/t_renderer b/third_party/acados/larch64/t_renderer
new file mode 100755
index 000000000..b4ff8bc31
Binary files /dev/null and b/third_party/acados/larch64/t_renderer differ
diff --git a/third_party/acados/x86_64/lib/libacados.so b/third_party/acados/x86_64/lib/libacados.so
new file mode 100644
index 000000000..5a1475426
Binary files /dev/null and b/third_party/acados/x86_64/lib/libacados.so differ
diff --git a/third_party/acados/x86_64/lib/libblasfeo.so b/third_party/acados/x86_64/lib/libblasfeo.so
new file mode 100644
index 000000000..5eb0e988c
Binary files /dev/null and b/third_party/acados/x86_64/lib/libblasfeo.so differ
diff --git a/third_party/acados/x86_64/lib/libhpipm.so b/third_party/acados/x86_64/lib/libhpipm.so
new file mode 100644
index 000000000..a647b056f
Binary files /dev/null and b/third_party/acados/x86_64/lib/libhpipm.so differ
diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so b/third_party/acados/x86_64/lib/libqpOASES_e.so
new file mode 120000
index 000000000..14048625a
--- /dev/null
+++ b/third_party/acados/x86_64/lib/libqpOASES_e.so
@@ -0,0 +1 @@
+libqpOASES_e.so.3.1
\ No newline at end of file
diff --git a/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1
new file mode 100644
index 000000000..6b553554f
Binary files /dev/null and b/third_party/acados/x86_64/lib/libqpOASES_e.so.3.1 differ
diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer
new file mode 100755
index 000000000..181ef9c3d
Binary files /dev/null and b/third_party/acados/x86_64/t_renderer differ
diff --git a/phonelibs/android_frameworks_native/get.txt b/third_party/android_frameworks_native/get.txt
similarity index 73%
rename from phonelibs/android_frameworks_native/get.txt
rename to third_party/android_frameworks_native/get.txt
index 31f42ebe0..277bdcb6a 100644
--- a/phonelibs/android_frameworks_native/get.txt
+++ b/third_party/android_frameworks_native/get.txt
@@ -1,3 +1,3 @@
 git clone https://github.com/CyanogenMod/android_frameworks_native.git && cd android_frameworks_native
 git reset --hard b22bca465e55618a949d9cbdea665a1a3a831241
-cp -r include ~/one/phonelibs/android_frameworks_native/
+cp -r include ~/one/third_party/android_frameworks_native/
diff --git a/phonelibs/android_frameworks_native/include/android/asset_manager.h b/third_party/android_frameworks_native/include/android/asset_manager.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/asset_manager.h
rename to third_party/android_frameworks_native/include/android/asset_manager.h
diff --git a/phonelibs/android_frameworks_native/include/android/asset_manager_jni.h b/third_party/android_frameworks_native/include/android/asset_manager_jni.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/asset_manager_jni.h
rename to third_party/android_frameworks_native/include/android/asset_manager_jni.h
diff --git a/phonelibs/android_frameworks_native/include/android/bitmap.h b/third_party/android_frameworks_native/include/android/bitmap.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/bitmap.h
rename to third_party/android_frameworks_native/include/android/bitmap.h
diff --git a/phonelibs/android_frameworks_native/include/android/configuration.h b/third_party/android_frameworks_native/include/android/configuration.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/configuration.h
rename to third_party/android_frameworks_native/include/android/configuration.h
diff --git a/phonelibs/android_frameworks_native/include/android/input.h b/third_party/android_frameworks_native/include/android/input.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/input.h
rename to third_party/android_frameworks_native/include/android/input.h
diff --git a/phonelibs/android_frameworks_native/include/android/keycodes.h b/third_party/android_frameworks_native/include/android/keycodes.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/keycodes.h
rename to third_party/android_frameworks_native/include/android/keycodes.h
diff --git a/phonelibs/android_frameworks_native/include/android/looper.h b/third_party/android_frameworks_native/include/android/looper.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/looper.h
rename to third_party/android_frameworks_native/include/android/looper.h
diff --git a/phonelibs/android_frameworks_native/include/android/multinetwork.h b/third_party/android_frameworks_native/include/android/multinetwork.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/multinetwork.h
rename to third_party/android_frameworks_native/include/android/multinetwork.h
diff --git a/phonelibs/android_frameworks_native/include/android/native_activity.h b/third_party/android_frameworks_native/include/android/native_activity.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/native_activity.h
rename to third_party/android_frameworks_native/include/android/native_activity.h
diff --git a/phonelibs/android_frameworks_native/include/android/native_window.h b/third_party/android_frameworks_native/include/android/native_window.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/native_window.h
rename to third_party/android_frameworks_native/include/android/native_window.h
diff --git a/phonelibs/android_frameworks_native/include/android/native_window_jni.h b/third_party/android_frameworks_native/include/android/native_window_jni.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/native_window_jni.h
rename to third_party/android_frameworks_native/include/android/native_window_jni.h
diff --git a/phonelibs/android_frameworks_native/include/android/obb.h b/third_party/android_frameworks_native/include/android/obb.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/obb.h
rename to third_party/android_frameworks_native/include/android/obb.h
diff --git a/phonelibs/android_frameworks_native/include/android/rect.h b/third_party/android_frameworks_native/include/android/rect.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/rect.h
rename to third_party/android_frameworks_native/include/android/rect.h
diff --git a/phonelibs/android_frameworks_native/include/android/sensor.h b/third_party/android_frameworks_native/include/android/sensor.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/sensor.h
rename to third_party/android_frameworks_native/include/android/sensor.h
diff --git a/phonelibs/android_frameworks_native/include/android/storage_manager.h b/third_party/android_frameworks_native/include/android/storage_manager.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/storage_manager.h
rename to third_party/android_frameworks_native/include/android/storage_manager.h
diff --git a/phonelibs/android_frameworks_native/include/android/trace.h b/third_party/android_frameworks_native/include/android/trace.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/trace.h
rename to third_party/android_frameworks_native/include/android/trace.h
diff --git a/phonelibs/android_frameworks_native/include/android/window.h b/third_party/android_frameworks_native/include/android/window.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/android/window.h
rename to third_party/android_frameworks_native/include/android/window.h
diff --git a/phonelibs/android_frameworks_native/include/binder/AppOpsManager.h b/third_party/android_frameworks_native/include/binder/AppOpsManager.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/AppOpsManager.h
rename to third_party/android_frameworks_native/include/binder/AppOpsManager.h
diff --git a/phonelibs/android_frameworks_native/include/binder/Binder.h b/third_party/android_frameworks_native/include/binder/Binder.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/Binder.h
rename to third_party/android_frameworks_native/include/binder/Binder.h
diff --git a/phonelibs/android_frameworks_native/include/binder/BinderService.h b/third_party/android_frameworks_native/include/binder/BinderService.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/BinderService.h
rename to third_party/android_frameworks_native/include/binder/BinderService.h
diff --git a/phonelibs/android_frameworks_native/include/binder/BpBinder.h b/third_party/android_frameworks_native/include/binder/BpBinder.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/BpBinder.h
rename to third_party/android_frameworks_native/include/binder/BpBinder.h
diff --git a/phonelibs/android_frameworks_native/include/binder/BufferedTextOutput.h b/third_party/android_frameworks_native/include/binder/BufferedTextOutput.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/BufferedTextOutput.h
rename to third_party/android_frameworks_native/include/binder/BufferedTextOutput.h
diff --git a/phonelibs/android_frameworks_native/include/binder/Debug.h b/third_party/android_frameworks_native/include/binder/Debug.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/Debug.h
rename to third_party/android_frameworks_native/include/binder/Debug.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IAppOpsCallback.h b/third_party/android_frameworks_native/include/binder/IAppOpsCallback.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IAppOpsCallback.h
rename to third_party/android_frameworks_native/include/binder/IAppOpsCallback.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IAppOpsService.h b/third_party/android_frameworks_native/include/binder/IAppOpsService.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IAppOpsService.h
rename to third_party/android_frameworks_native/include/binder/IAppOpsService.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IBatteryStats.h b/third_party/android_frameworks_native/include/binder/IBatteryStats.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IBatteryStats.h
rename to third_party/android_frameworks_native/include/binder/IBatteryStats.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IBinder.h b/third_party/android_frameworks_native/include/binder/IBinder.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IBinder.h
rename to third_party/android_frameworks_native/include/binder/IBinder.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IInterface.h b/third_party/android_frameworks_native/include/binder/IInterface.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IInterface.h
rename to third_party/android_frameworks_native/include/binder/IInterface.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IMemory.h b/third_party/android_frameworks_native/include/binder/IMemory.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IMemory.h
rename to third_party/android_frameworks_native/include/binder/IMemory.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IPCThreadState.h b/third_party/android_frameworks_native/include/binder/IPCThreadState.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IPCThreadState.h
rename to third_party/android_frameworks_native/include/binder/IPCThreadState.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IPermissionController.h b/third_party/android_frameworks_native/include/binder/IPermissionController.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IPermissionController.h
rename to third_party/android_frameworks_native/include/binder/IPermissionController.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IProcessInfoService.h b/third_party/android_frameworks_native/include/binder/IProcessInfoService.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IProcessInfoService.h
rename to third_party/android_frameworks_native/include/binder/IProcessInfoService.h
diff --git a/phonelibs/android_frameworks_native/include/binder/IServiceManager.h b/third_party/android_frameworks_native/include/binder/IServiceManager.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/IServiceManager.h
rename to third_party/android_frameworks_native/include/binder/IServiceManager.h
diff --git a/phonelibs/android_frameworks_native/include/binder/MemoryBase.h b/third_party/android_frameworks_native/include/binder/MemoryBase.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/MemoryBase.h
rename to third_party/android_frameworks_native/include/binder/MemoryBase.h
diff --git a/phonelibs/android_frameworks_native/include/binder/MemoryDealer.h b/third_party/android_frameworks_native/include/binder/MemoryDealer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/MemoryDealer.h
rename to third_party/android_frameworks_native/include/binder/MemoryDealer.h
diff --git a/phonelibs/android_frameworks_native/include/binder/MemoryHeapBase.h b/third_party/android_frameworks_native/include/binder/MemoryHeapBase.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/MemoryHeapBase.h
rename to third_party/android_frameworks_native/include/binder/MemoryHeapBase.h
diff --git a/phonelibs/android_frameworks_native/include/binder/MemoryHeapIon.h b/third_party/android_frameworks_native/include/binder/MemoryHeapIon.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/MemoryHeapIon.h
rename to third_party/android_frameworks_native/include/binder/MemoryHeapIon.h
diff --git a/phonelibs/android_frameworks_native/include/binder/Parcel.h b/third_party/android_frameworks_native/include/binder/Parcel.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/Parcel.h
rename to third_party/android_frameworks_native/include/binder/Parcel.h
diff --git a/phonelibs/android_frameworks_native/include/binder/PermissionCache.h b/third_party/android_frameworks_native/include/binder/PermissionCache.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/PermissionCache.h
rename to third_party/android_frameworks_native/include/binder/PermissionCache.h
diff --git a/phonelibs/android_frameworks_native/include/binder/ProcessInfoService.h b/third_party/android_frameworks_native/include/binder/ProcessInfoService.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/ProcessInfoService.h
rename to third_party/android_frameworks_native/include/binder/ProcessInfoService.h
diff --git a/phonelibs/android_frameworks_native/include/binder/ProcessState.h b/third_party/android_frameworks_native/include/binder/ProcessState.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/ProcessState.h
rename to third_party/android_frameworks_native/include/binder/ProcessState.h
diff --git a/phonelibs/android_frameworks_native/include/binder/TextOutput.h b/third_party/android_frameworks_native/include/binder/TextOutput.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/binder/TextOutput.h
rename to third_party/android_frameworks_native/include/binder/TextOutput.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BitTube.h b/third_party/android_frameworks_native/include/gui/BitTube.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BitTube.h
rename to third_party/android_frameworks_native/include/gui/BitTube.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferItem.h b/third_party/android_frameworks_native/include/gui/BufferItem.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferItem.h
rename to third_party/android_frameworks_native/include/gui/BufferItem.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferItemConsumer.h b/third_party/android_frameworks_native/include/gui/BufferItemConsumer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferItemConsumer.h
rename to third_party/android_frameworks_native/include/gui/BufferItemConsumer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferQueue.h b/third_party/android_frameworks_native/include/gui/BufferQueue.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferQueue.h
rename to third_party/android_frameworks_native/include/gui/BufferQueue.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferQueueConsumer.h b/third_party/android_frameworks_native/include/gui/BufferQueueConsumer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferQueueConsumer.h
rename to third_party/android_frameworks_native/include/gui/BufferQueueConsumer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferQueueCore.h b/third_party/android_frameworks_native/include/gui/BufferQueueCore.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferQueueCore.h
rename to third_party/android_frameworks_native/include/gui/BufferQueueCore.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferQueueDefs.h b/third_party/android_frameworks_native/include/gui/BufferQueueDefs.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferQueueDefs.h
rename to third_party/android_frameworks_native/include/gui/BufferQueueDefs.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferQueueProducer.h b/third_party/android_frameworks_native/include/gui/BufferQueueProducer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferQueueProducer.h
rename to third_party/android_frameworks_native/include/gui/BufferQueueProducer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/BufferSlot.h b/third_party/android_frameworks_native/include/gui/BufferSlot.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/BufferSlot.h
rename to third_party/android_frameworks_native/include/gui/BufferSlot.h
diff --git a/phonelibs/android_frameworks_native/include/gui/ConsumerBase.h b/third_party/android_frameworks_native/include/gui/ConsumerBase.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/ConsumerBase.h
rename to third_party/android_frameworks_native/include/gui/ConsumerBase.h
diff --git a/phonelibs/android_frameworks_native/include/gui/CpuConsumer.h b/third_party/android_frameworks_native/include/gui/CpuConsumer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/CpuConsumer.h
rename to third_party/android_frameworks_native/include/gui/CpuConsumer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/DisplayEventReceiver.h b/third_party/android_frameworks_native/include/gui/DisplayEventReceiver.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/DisplayEventReceiver.h
rename to third_party/android_frameworks_native/include/gui/DisplayEventReceiver.h
diff --git a/phonelibs/android_frameworks_native/include/gui/GLConsumer.h b/third_party/android_frameworks_native/include/gui/GLConsumer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/GLConsumer.h
rename to third_party/android_frameworks_native/include/gui/GLConsumer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/GraphicBufferAlloc.h b/third_party/android_frameworks_native/include/gui/GraphicBufferAlloc.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/GraphicBufferAlloc.h
rename to third_party/android_frameworks_native/include/gui/GraphicBufferAlloc.h
diff --git a/phonelibs/android_frameworks_native/include/gui/GuiConfig.h b/third_party/android_frameworks_native/include/gui/GuiConfig.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/GuiConfig.h
rename to third_party/android_frameworks_native/include/gui/GuiConfig.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IConsumerListener.h b/third_party/android_frameworks_native/include/gui/IConsumerListener.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IConsumerListener.h
rename to third_party/android_frameworks_native/include/gui/IConsumerListener.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IDisplayEventConnection.h b/third_party/android_frameworks_native/include/gui/IDisplayEventConnection.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IDisplayEventConnection.h
rename to third_party/android_frameworks_native/include/gui/IDisplayEventConnection.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IGraphicBufferAlloc.h b/third_party/android_frameworks_native/include/gui/IGraphicBufferAlloc.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IGraphicBufferAlloc.h
rename to third_party/android_frameworks_native/include/gui/IGraphicBufferAlloc.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IGraphicBufferConsumer.h b/third_party/android_frameworks_native/include/gui/IGraphicBufferConsumer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IGraphicBufferConsumer.h
rename to third_party/android_frameworks_native/include/gui/IGraphicBufferConsumer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IGraphicBufferProducer.h b/third_party/android_frameworks_native/include/gui/IGraphicBufferProducer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IGraphicBufferProducer.h
rename to third_party/android_frameworks_native/include/gui/IGraphicBufferProducer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/IProducerListener.h b/third_party/android_frameworks_native/include/gui/IProducerListener.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/IProducerListener.h
rename to third_party/android_frameworks_native/include/gui/IProducerListener.h
diff --git a/phonelibs/android_frameworks_native/include/gui/ISensorEventConnection.h b/third_party/android_frameworks_native/include/gui/ISensorEventConnection.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/ISensorEventConnection.h
rename to third_party/android_frameworks_native/include/gui/ISensorEventConnection.h
diff --git a/phonelibs/android_frameworks_native/include/gui/ISensorServer.h b/third_party/android_frameworks_native/include/gui/ISensorServer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/ISensorServer.h
rename to third_party/android_frameworks_native/include/gui/ISensorServer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/ISurfaceComposer.h b/third_party/android_frameworks_native/include/gui/ISurfaceComposer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/ISurfaceComposer.h
rename to third_party/android_frameworks_native/include/gui/ISurfaceComposer.h
diff --git a/phonelibs/android_frameworks_native/include/gui/ISurfaceComposerClient.h b/third_party/android_frameworks_native/include/gui/ISurfaceComposerClient.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/ISurfaceComposerClient.h
rename to third_party/android_frameworks_native/include/gui/ISurfaceComposerClient.h
diff --git a/phonelibs/android_frameworks_native/include/gui/Sensor.h b/third_party/android_frameworks_native/include/gui/Sensor.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/Sensor.h
rename to third_party/android_frameworks_native/include/gui/Sensor.h
diff --git a/phonelibs/android_frameworks_native/include/gui/SensorEventQueue.h b/third_party/android_frameworks_native/include/gui/SensorEventQueue.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/SensorEventQueue.h
rename to third_party/android_frameworks_native/include/gui/SensorEventQueue.h
diff --git a/phonelibs/android_frameworks_native/include/gui/SensorManager.h b/third_party/android_frameworks_native/include/gui/SensorManager.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/SensorManager.h
rename to third_party/android_frameworks_native/include/gui/SensorManager.h
diff --git a/phonelibs/android_frameworks_native/include/gui/StreamSplitter.h b/third_party/android_frameworks_native/include/gui/StreamSplitter.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/StreamSplitter.h
rename to third_party/android_frameworks_native/include/gui/StreamSplitter.h
diff --git a/phonelibs/android_frameworks_native/include/gui/Surface.h b/third_party/android_frameworks_native/include/gui/Surface.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/Surface.h
rename to third_party/android_frameworks_native/include/gui/Surface.h
diff --git a/phonelibs/android_frameworks_native/include/gui/SurfaceComposerClient.h b/third_party/android_frameworks_native/include/gui/SurfaceComposerClient.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/SurfaceComposerClient.h
rename to third_party/android_frameworks_native/include/gui/SurfaceComposerClient.h
diff --git a/phonelibs/android_frameworks_native/include/gui/SurfaceControl.h b/third_party/android_frameworks_native/include/gui/SurfaceControl.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/gui/SurfaceControl.h
rename to third_party/android_frameworks_native/include/gui/SurfaceControl.h
diff --git a/phonelibs/android_frameworks_native/include/ui/ANativeObjectBase.h b/third_party/android_frameworks_native/include/ui/ANativeObjectBase.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/ANativeObjectBase.h
rename to third_party/android_frameworks_native/include/ui/ANativeObjectBase.h
diff --git a/phonelibs/android_frameworks_native/include/ui/DisplayInfo.h b/third_party/android_frameworks_native/include/ui/DisplayInfo.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/DisplayInfo.h
rename to third_party/android_frameworks_native/include/ui/DisplayInfo.h
diff --git a/phonelibs/android_frameworks_native/include/ui/DisplayStatInfo.h b/third_party/android_frameworks_native/include/ui/DisplayStatInfo.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/DisplayStatInfo.h
rename to third_party/android_frameworks_native/include/ui/DisplayStatInfo.h
diff --git a/phonelibs/android_frameworks_native/include/ui/Fence.h b/third_party/android_frameworks_native/include/ui/Fence.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/Fence.h
rename to third_party/android_frameworks_native/include/ui/Fence.h
diff --git a/phonelibs/android_frameworks_native/include/ui/FrameStats.h b/third_party/android_frameworks_native/include/ui/FrameStats.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/FrameStats.h
rename to third_party/android_frameworks_native/include/ui/FrameStats.h
diff --git a/phonelibs/android_frameworks_native/include/ui/FramebufferNativeWindow.h b/third_party/android_frameworks_native/include/ui/FramebufferNativeWindow.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/FramebufferNativeWindow.h
rename to third_party/android_frameworks_native/include/ui/FramebufferNativeWindow.h
diff --git a/phonelibs/android_frameworks_native/include/ui/GraphicBuffer.h b/third_party/android_frameworks_native/include/ui/GraphicBuffer.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/GraphicBuffer.h
rename to third_party/android_frameworks_native/include/ui/GraphicBuffer.h
diff --git a/phonelibs/android_frameworks_native/include/ui/GraphicBufferAllocator.h b/third_party/android_frameworks_native/include/ui/GraphicBufferAllocator.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/GraphicBufferAllocator.h
rename to third_party/android_frameworks_native/include/ui/GraphicBufferAllocator.h
diff --git a/phonelibs/android_frameworks_native/include/ui/GraphicBufferMapper.h b/third_party/android_frameworks_native/include/ui/GraphicBufferMapper.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/GraphicBufferMapper.h
rename to third_party/android_frameworks_native/include/ui/GraphicBufferMapper.h
diff --git a/phonelibs/android_frameworks_native/include/ui/PixelFormat.h b/third_party/android_frameworks_native/include/ui/PixelFormat.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/PixelFormat.h
rename to third_party/android_frameworks_native/include/ui/PixelFormat.h
diff --git a/phonelibs/android_frameworks_native/include/ui/Point.h b/third_party/android_frameworks_native/include/ui/Point.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/Point.h
rename to third_party/android_frameworks_native/include/ui/Point.h
diff --git a/phonelibs/android_frameworks_native/include/ui/Rect.h b/third_party/android_frameworks_native/include/ui/Rect.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/Rect.h
rename to third_party/android_frameworks_native/include/ui/Rect.h
diff --git a/phonelibs/android_frameworks_native/include/ui/Region.h b/third_party/android_frameworks_native/include/ui/Region.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/Region.h
rename to third_party/android_frameworks_native/include/ui/Region.h
diff --git a/phonelibs/android_frameworks_native/include/ui/TMatHelpers.h b/third_party/android_frameworks_native/include/ui/TMatHelpers.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/TMatHelpers.h
rename to third_party/android_frameworks_native/include/ui/TMatHelpers.h
diff --git a/phonelibs/android_frameworks_native/include/ui/TVecHelpers.h b/third_party/android_frameworks_native/include/ui/TVecHelpers.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/TVecHelpers.h
rename to third_party/android_frameworks_native/include/ui/TVecHelpers.h
diff --git a/phonelibs/android_frameworks_native/include/ui/UiConfig.h b/third_party/android_frameworks_native/include/ui/UiConfig.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/UiConfig.h
rename to third_party/android_frameworks_native/include/ui/UiConfig.h
diff --git a/phonelibs/android_frameworks_native/include/ui/mat4.h b/third_party/android_frameworks_native/include/ui/mat4.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/mat4.h
rename to third_party/android_frameworks_native/include/ui/mat4.h
diff --git a/phonelibs/android_frameworks_native/include/ui/vec2.h b/third_party/android_frameworks_native/include/ui/vec2.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/vec2.h
rename to third_party/android_frameworks_native/include/ui/vec2.h
diff --git a/phonelibs/android_frameworks_native/include/ui/vec3.h b/third_party/android_frameworks_native/include/ui/vec3.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/vec3.h
rename to third_party/android_frameworks_native/include/ui/vec3.h
diff --git a/phonelibs/android_frameworks_native/include/ui/vec4.h b/third_party/android_frameworks_native/include/ui/vec4.h
similarity index 100%
rename from phonelibs/android_frameworks_native/include/ui/vec4.h
rename to third_party/android_frameworks_native/include/ui/vec4.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/activity_recognition.h b/third_party/android_hardware_libhardware/include/hardware/activity_recognition.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/activity_recognition.h
rename to third_party/android_hardware_libhardware/include/hardware/activity_recognition.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/audio.h b/third_party/android_hardware_libhardware/include/hardware/audio.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/audio.h
rename to third_party/android_hardware_libhardware/include/hardware/audio.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/audio_alsaops.h b/third_party/android_hardware_libhardware/include/hardware/audio_alsaops.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/audio_alsaops.h
rename to third_party/android_hardware_libhardware/include/hardware/audio_alsaops.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/audio_amplifier.h b/third_party/android_hardware_libhardware/include/hardware/audio_amplifier.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/audio_amplifier.h
rename to third_party/android_hardware_libhardware/include/hardware/audio_amplifier.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/audio_effect.h b/third_party/android_hardware_libhardware/include/hardware/audio_effect.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/audio_effect.h
rename to third_party/android_hardware_libhardware/include/hardware/audio_effect.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/audio_policy.h b/third_party/android_hardware_libhardware/include/hardware/audio_policy.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/audio_policy.h
rename to third_party/android_hardware_libhardware/include/hardware/audio_policy.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bluetooth.h b/third_party/android_hardware_libhardware/include/hardware/bluetooth.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bluetooth.h
rename to third_party/android_hardware_libhardware/include/hardware/bluetooth.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_av.h b/third_party/android_hardware_libhardware/include/hardware/bt_av.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_av.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_av.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_common_types.h b/third_party/android_hardware_libhardware/include/hardware/bt_common_types.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_common_types.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_common_types.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_gatt.h b/third_party/android_hardware_libhardware/include/hardware/bt_gatt.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_gatt.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_gatt.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_client.h b/third_party/android_hardware_libhardware/include/hardware/bt_gatt_client.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_client.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_gatt_client.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_server.h b/third_party/android_hardware_libhardware/include/hardware/bt_gatt_server.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_server.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_gatt_server.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_types.h b/third_party/android_hardware_libhardware/include/hardware/bt_gatt_types.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_gatt_types.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_gatt_types.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_hd.h b/third_party/android_hardware_libhardware/include/hardware/bt_hd.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_hd.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_hd.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_hf.h b/third_party/android_hardware_libhardware/include/hardware/bt_hf.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_hf.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_hf.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_hf_client.h b/third_party/android_hardware_libhardware/include/hardware/bt_hf_client.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_hf_client.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_hf_client.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_hh.h b/third_party/android_hardware_libhardware/include/hardware/bt_hh.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_hh.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_hh.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_hl.h b/third_party/android_hardware_libhardware/include/hardware/bt_hl.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_hl.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_hl.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_mce.h b/third_party/android_hardware_libhardware/include/hardware/bt_mce.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_mce.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_mce.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_pan.h b/third_party/android_hardware_libhardware/include/hardware/bt_pan.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_pan.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_pan.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_rc.h b/third_party/android_hardware_libhardware/include/hardware/bt_rc.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_rc.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_rc.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_sdp.h b/third_party/android_hardware_libhardware/include/hardware/bt_sdp.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_sdp.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_sdp.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/bt_sock.h b/third_party/android_hardware_libhardware/include/hardware/bt_sock.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/bt_sock.h
rename to third_party/android_hardware_libhardware/include/hardware/bt_sock.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/camera.h b/third_party/android_hardware_libhardware/include/hardware/camera.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/camera.h
rename to third_party/android_hardware_libhardware/include/hardware/camera.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/camera2.h b/third_party/android_hardware_libhardware/include/hardware/camera2.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/camera2.h
rename to third_party/android_hardware_libhardware/include/hardware/camera2.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/camera3.h b/third_party/android_hardware_libhardware/include/hardware/camera3.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/camera3.h
rename to third_party/android_hardware_libhardware/include/hardware/camera3.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/camera_common.h b/third_party/android_hardware_libhardware/include/hardware/camera_common.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/camera_common.h
rename to third_party/android_hardware_libhardware/include/hardware/camera_common.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/consumerir.h b/third_party/android_hardware_libhardware/include/hardware/consumerir.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/consumerir.h
rename to third_party/android_hardware_libhardware/include/hardware/consumerir.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/display_defs.h b/third_party/android_hardware_libhardware/include/hardware/display_defs.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/display_defs.h
rename to third_party/android_hardware_libhardware/include/hardware/display_defs.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/fb.h b/third_party/android_hardware_libhardware/include/hardware/fb.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/fb.h
rename to third_party/android_hardware_libhardware/include/hardware/fb.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/fingerprint.h b/third_party/android_hardware_libhardware/include/hardware/fingerprint.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/fingerprint.h
rename to third_party/android_hardware_libhardware/include/hardware/fingerprint.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/fused_location.h b/third_party/android_hardware_libhardware/include/hardware/fused_location.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/fused_location.h
rename to third_party/android_hardware_libhardware/include/hardware/fused_location.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/gatekeeper.h b/third_party/android_hardware_libhardware/include/hardware/gatekeeper.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/gatekeeper.h
rename to third_party/android_hardware_libhardware/include/hardware/gatekeeper.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/gps.h b/third_party/android_hardware_libhardware/include/hardware/gps.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/gps.h
rename to third_party/android_hardware_libhardware/include/hardware/gps.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/gralloc.h b/third_party/android_hardware_libhardware/include/hardware/gralloc.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/gralloc.h
rename to third_party/android_hardware_libhardware/include/hardware/gralloc.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/hardware.h b/third_party/android_hardware_libhardware/include/hardware/hardware.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/hardware.h
rename to third_party/android_hardware_libhardware/include/hardware/hardware.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/hdmi_cec.h b/third_party/android_hardware_libhardware/include/hardware/hdmi_cec.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/hdmi_cec.h
rename to third_party/android_hardware_libhardware/include/hardware/hdmi_cec.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/hw_auth_token.h b/third_party/android_hardware_libhardware/include/hardware/hw_auth_token.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/hw_auth_token.h
rename to third_party/android_hardware_libhardware/include/hardware/hw_auth_token.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/hwcomposer.h b/third_party/android_hardware_libhardware/include/hardware/hwcomposer.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/hwcomposer.h
rename to third_party/android_hardware_libhardware/include/hardware/hwcomposer.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/hwcomposer_defs.h b/third_party/android_hardware_libhardware/include/hardware/hwcomposer_defs.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/hwcomposer_defs.h
rename to third_party/android_hardware_libhardware/include/hardware/hwcomposer_defs.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/input.h b/third_party/android_hardware_libhardware/include/hardware/input.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/input.h
rename to third_party/android_hardware_libhardware/include/hardware/input.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/keymaster0.h b/third_party/android_hardware_libhardware/include/hardware/keymaster0.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/keymaster0.h
rename to third_party/android_hardware_libhardware/include/hardware/keymaster0.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/keymaster1.h b/third_party/android_hardware_libhardware/include/hardware/keymaster1.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/keymaster1.h
rename to third_party/android_hardware_libhardware/include/hardware/keymaster1.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/keymaster_common.h b/third_party/android_hardware_libhardware/include/hardware/keymaster_common.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/keymaster_common.h
rename to third_party/android_hardware_libhardware/include/hardware/keymaster_common.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/keymaster_defs.h b/third_party/android_hardware_libhardware/include/hardware/keymaster_defs.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/keymaster_defs.h
rename to third_party/android_hardware_libhardware/include/hardware/keymaster_defs.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/lights.h b/third_party/android_hardware_libhardware/include/hardware/lights.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/lights.h
rename to third_party/android_hardware_libhardware/include/hardware/lights.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/local_time_hal.h b/third_party/android_hardware_libhardware/include/hardware/local_time_hal.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/local_time_hal.h
rename to third_party/android_hardware_libhardware/include/hardware/local_time_hal.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/memtrack.h b/third_party/android_hardware_libhardware/include/hardware/memtrack.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/memtrack.h
rename to third_party/android_hardware_libhardware/include/hardware/memtrack.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/nfc.h b/third_party/android_hardware_libhardware/include/hardware/nfc.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/nfc.h
rename to third_party/android_hardware_libhardware/include/hardware/nfc.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/nfc_tag.h b/third_party/android_hardware_libhardware/include/hardware/nfc_tag.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/nfc_tag.h
rename to third_party/android_hardware_libhardware/include/hardware/nfc_tag.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/power.h b/third_party/android_hardware_libhardware/include/hardware/power.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/power.h
rename to third_party/android_hardware_libhardware/include/hardware/power.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/qemu_pipe.h b/third_party/android_hardware_libhardware/include/hardware/qemu_pipe.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/qemu_pipe.h
rename to third_party/android_hardware_libhardware/include/hardware/qemu_pipe.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/qemud.h b/third_party/android_hardware_libhardware/include/hardware/qemud.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/qemud.h
rename to third_party/android_hardware_libhardware/include/hardware/qemud.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/radio.h b/third_party/android_hardware_libhardware/include/hardware/radio.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/radio.h
rename to third_party/android_hardware_libhardware/include/hardware/radio.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/sensors.h b/third_party/android_hardware_libhardware/include/hardware/sensors.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/sensors.h
rename to third_party/android_hardware_libhardware/include/hardware/sensors.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/sound_trigger.h b/third_party/android_hardware_libhardware/include/hardware/sound_trigger.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/sound_trigger.h
rename to third_party/android_hardware_libhardware/include/hardware/sound_trigger.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/tv_input.h b/third_party/android_hardware_libhardware/include/hardware/tv_input.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/tv_input.h
rename to third_party/android_hardware_libhardware/include/hardware/tv_input.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/vibrator.h b/third_party/android_hardware_libhardware/include/hardware/vibrator.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/vibrator.h
rename to third_party/android_hardware_libhardware/include/hardware/vibrator.h
diff --git a/phonelibs/android_hardware_libhardware/include/hardware/wipower.h b/third_party/android_hardware_libhardware/include/hardware/wipower.h
similarity index 100%
rename from phonelibs/android_hardware_libhardware/include/hardware/wipower.h
rename to third_party/android_hardware_libhardware/include/hardware/wipower.h
diff --git a/phonelibs/android_system_core/include/cutils/android_reboot.h b/third_party/android_system_core/include/cutils/android_reboot.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/android_reboot.h
rename to third_party/android_system_core/include/cutils/android_reboot.h
diff --git a/phonelibs/android_system_core/include/cutils/aref.h b/third_party/android_system_core/include/cutils/aref.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/aref.h
rename to third_party/android_system_core/include/cutils/aref.h
diff --git a/phonelibs/android_system_core/include/cutils/ashmem.h b/third_party/android_system_core/include/cutils/ashmem.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/ashmem.h
rename to third_party/android_system_core/include/cutils/ashmem.h
diff --git a/phonelibs/android_system_core/include/cutils/atomic.h b/third_party/android_system_core/include/cutils/atomic.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/atomic.h
rename to third_party/android_system_core/include/cutils/atomic.h
diff --git a/phonelibs/android_system_core/include/cutils/bitops.h b/third_party/android_system_core/include/cutils/bitops.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/bitops.h
rename to third_party/android_system_core/include/cutils/bitops.h
diff --git a/phonelibs/android_system_core/include/cutils/compiler.h b/third_party/android_system_core/include/cutils/compiler.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/compiler.h
rename to third_party/android_system_core/include/cutils/compiler.h
diff --git a/phonelibs/android_system_core/include/cutils/config_utils.h b/third_party/android_system_core/include/cutils/config_utils.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/config_utils.h
rename to third_party/android_system_core/include/cutils/config_utils.h
diff --git a/phonelibs/android_system_core/include/cutils/debugger.h b/third_party/android_system_core/include/cutils/debugger.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/debugger.h
rename to third_party/android_system_core/include/cutils/debugger.h
diff --git a/phonelibs/android_system_core/include/cutils/fs.h b/third_party/android_system_core/include/cutils/fs.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/fs.h
rename to third_party/android_system_core/include/cutils/fs.h
diff --git a/phonelibs/android_system_core/include/cutils/hashmap.h b/third_party/android_system_core/include/cutils/hashmap.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/hashmap.h
rename to third_party/android_system_core/include/cutils/hashmap.h
diff --git a/phonelibs/android_system_core/include/cutils/iosched_policy.h b/third_party/android_system_core/include/cutils/iosched_policy.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/iosched_policy.h
rename to third_party/android_system_core/include/cutils/iosched_policy.h
diff --git a/phonelibs/android_system_core/include/cutils/jstring.h b/third_party/android_system_core/include/cutils/jstring.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/jstring.h
rename to third_party/android_system_core/include/cutils/jstring.h
diff --git a/phonelibs/android_system_core/include/cutils/klog.h b/third_party/android_system_core/include/cutils/klog.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/klog.h
rename to third_party/android_system_core/include/cutils/klog.h
diff --git a/phonelibs/android_system_core/include/cutils/list.h b/third_party/android_system_core/include/cutils/list.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/list.h
rename to third_party/android_system_core/include/cutils/list.h
diff --git a/phonelibs/android_system_core/include/cutils/log.h b/third_party/android_system_core/include/cutils/log.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/log.h
rename to third_party/android_system_core/include/cutils/log.h
diff --git a/phonelibs/android_system_core/include/cutils/memory.h b/third_party/android_system_core/include/cutils/memory.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/memory.h
rename to third_party/android_system_core/include/cutils/memory.h
diff --git a/phonelibs/android_system_core/include/cutils/misc.h b/third_party/android_system_core/include/cutils/misc.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/misc.h
rename to third_party/android_system_core/include/cutils/misc.h
diff --git a/phonelibs/android_system_core/include/cutils/multiuser.h b/third_party/android_system_core/include/cutils/multiuser.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/multiuser.h
rename to third_party/android_system_core/include/cutils/multiuser.h
diff --git a/phonelibs/android_system_core/include/cutils/native_handle.h b/third_party/android_system_core/include/cutils/native_handle.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/native_handle.h
rename to third_party/android_system_core/include/cutils/native_handle.h
diff --git a/phonelibs/android_system_core/include/cutils/open_memstream.h b/third_party/android_system_core/include/cutils/open_memstream.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/open_memstream.h
rename to third_party/android_system_core/include/cutils/open_memstream.h
diff --git a/phonelibs/android_system_core/include/cutils/partition_utils.h b/third_party/android_system_core/include/cutils/partition_utils.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/partition_utils.h
rename to third_party/android_system_core/include/cutils/partition_utils.h
diff --git a/phonelibs/android_system_core/include/cutils/process_name.h b/third_party/android_system_core/include/cutils/process_name.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/process_name.h
rename to third_party/android_system_core/include/cutils/process_name.h
diff --git a/phonelibs/android_system_core/include/cutils/properties.h b/third_party/android_system_core/include/cutils/properties.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/properties.h
rename to third_party/android_system_core/include/cutils/properties.h
diff --git a/phonelibs/android_system_core/include/cutils/qtaguid.h b/third_party/android_system_core/include/cutils/qtaguid.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/qtaguid.h
rename to third_party/android_system_core/include/cutils/qtaguid.h
diff --git a/phonelibs/android_system_core/include/cutils/record_stream.h b/third_party/android_system_core/include/cutils/record_stream.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/record_stream.h
rename to third_party/android_system_core/include/cutils/record_stream.h
diff --git a/phonelibs/android_system_core/include/cutils/sched_policy.h b/third_party/android_system_core/include/cutils/sched_policy.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/sched_policy.h
rename to third_party/android_system_core/include/cutils/sched_policy.h
diff --git a/phonelibs/android_system_core/include/cutils/sockets.h b/third_party/android_system_core/include/cutils/sockets.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/sockets.h
rename to third_party/android_system_core/include/cutils/sockets.h
diff --git a/phonelibs/android_system_core/include/cutils/str_parms.h b/third_party/android_system_core/include/cutils/str_parms.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/str_parms.h
rename to third_party/android_system_core/include/cutils/str_parms.h
diff --git a/phonelibs/android_system_core/include/cutils/threads.h b/third_party/android_system_core/include/cutils/threads.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/threads.h
rename to third_party/android_system_core/include/cutils/threads.h
diff --git a/phonelibs/android_system_core/include/cutils/trace.h b/third_party/android_system_core/include/cutils/trace.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/trace.h
rename to third_party/android_system_core/include/cutils/trace.h
diff --git a/phonelibs/android_system_core/include/cutils/uevent.h b/third_party/android_system_core/include/cutils/uevent.h
similarity index 100%
rename from phonelibs/android_system_core/include/cutils/uevent.h
rename to third_party/android_system_core/include/cutils/uevent.h
diff --git a/phonelibs/android_system_core/include/log/event_tag_map.h b/third_party/android_system_core/include/log/event_tag_map.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/event_tag_map.h
rename to third_party/android_system_core/include/log/event_tag_map.h
diff --git a/phonelibs/android_system_core/include/log/log.h b/third_party/android_system_core/include/log/log.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/log.h
rename to third_party/android_system_core/include/log/log.h
diff --git a/phonelibs/android_system_core/include/log/log_read.h b/third_party/android_system_core/include/log/log_read.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/log_read.h
rename to third_party/android_system_core/include/log/log_read.h
diff --git a/phonelibs/android_system_core/include/log/logd.h b/third_party/android_system_core/include/log/logd.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/logd.h
rename to third_party/android_system_core/include/log/logd.h
diff --git a/phonelibs/android_system_core/include/log/logger.h b/third_party/android_system_core/include/log/logger.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/logger.h
rename to third_party/android_system_core/include/log/logger.h
diff --git a/phonelibs/android_system_core/include/log/logprint.h b/third_party/android_system_core/include/log/logprint.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/logprint.h
rename to third_party/android_system_core/include/log/logprint.h
diff --git a/phonelibs/android_system_core/include/log/uio.h b/third_party/android_system_core/include/log/uio.h
similarity index 100%
rename from phonelibs/android_system_core/include/log/uio.h
rename to third_party/android_system_core/include/log/uio.h
diff --git a/phonelibs/android_system_core/include/system/camera.h b/third_party/android_system_core/include/system/camera.h
similarity index 100%
rename from phonelibs/android_system_core/include/system/camera.h
rename to third_party/android_system_core/include/system/camera.h
diff --git a/phonelibs/android_system_core/include/system/graphics.h b/third_party/android_system_core/include/system/graphics.h
similarity index 100%
rename from phonelibs/android_system_core/include/system/graphics.h
rename to third_party/android_system_core/include/system/graphics.h
diff --git a/phonelibs/android_system_core/include/system/radio.h b/third_party/android_system_core/include/system/radio.h
similarity index 100%
rename from phonelibs/android_system_core/include/system/radio.h
rename to third_party/android_system_core/include/system/radio.h
diff --git a/phonelibs/android_system_core/include/system/thread_defs.h b/third_party/android_system_core/include/system/thread_defs.h
similarity index 100%
rename from phonelibs/android_system_core/include/system/thread_defs.h
rename to third_party/android_system_core/include/system/thread_defs.h
diff --git a/phonelibs/android_system_core/include/system/window.h b/third_party/android_system_core/include/system/window.h
similarity index 100%
rename from phonelibs/android_system_core/include/system/window.h
rename to third_party/android_system_core/include/system/window.h
diff --git a/phonelibs/android_system_core/include/utils/AndroidThreads.h b/third_party/android_system_core/include/utils/AndroidThreads.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/AndroidThreads.h
rename to third_party/android_system_core/include/utils/AndroidThreads.h
diff --git a/phonelibs/android_system_core/include/utils/Atomic.h b/third_party/android_system_core/include/utils/Atomic.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Atomic.h
rename to third_party/android_system_core/include/utils/Atomic.h
diff --git a/phonelibs/android_system_core/include/utils/BasicHashtable.h b/third_party/android_system_core/include/utils/BasicHashtable.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/BasicHashtable.h
rename to third_party/android_system_core/include/utils/BasicHashtable.h
diff --git a/phonelibs/android_system_core/include/utils/BitSet.h b/third_party/android_system_core/include/utils/BitSet.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/BitSet.h
rename to third_party/android_system_core/include/utils/BitSet.h
diff --git a/phonelibs/android_system_core/include/utils/BlobCache.h b/third_party/android_system_core/include/utils/BlobCache.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/BlobCache.h
rename to third_party/android_system_core/include/utils/BlobCache.h
diff --git a/phonelibs/android_system_core/include/utils/ByteOrder.h b/third_party/android_system_core/include/utils/ByteOrder.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/ByteOrder.h
rename to third_party/android_system_core/include/utils/ByteOrder.h
diff --git a/phonelibs/android_system_core/include/utils/CallStack.h b/third_party/android_system_core/include/utils/CallStack.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/CallStack.h
rename to third_party/android_system_core/include/utils/CallStack.h
diff --git a/phonelibs/android_system_core/include/utils/Compat.h b/third_party/android_system_core/include/utils/Compat.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Compat.h
rename to third_party/android_system_core/include/utils/Compat.h
diff --git a/phonelibs/android_system_core/include/utils/Condition.h b/third_party/android_system_core/include/utils/Condition.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Condition.h
rename to third_party/android_system_core/include/utils/Condition.h
diff --git a/phonelibs/android_system_core/include/utils/Debug.h b/third_party/android_system_core/include/utils/Debug.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Debug.h
rename to third_party/android_system_core/include/utils/Debug.h
diff --git a/phonelibs/android_system_core/include/utils/Endian.h b/third_party/android_system_core/include/utils/Endian.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Endian.h
rename to third_party/android_system_core/include/utils/Endian.h
diff --git a/phonelibs/android_system_core/include/utils/Errors.h b/third_party/android_system_core/include/utils/Errors.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Errors.h
rename to third_party/android_system_core/include/utils/Errors.h
diff --git a/phonelibs/android_system_core/include/utils/FileMap.h b/third_party/android_system_core/include/utils/FileMap.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/FileMap.h
rename to third_party/android_system_core/include/utils/FileMap.h
diff --git a/phonelibs/android_system_core/include/utils/Flattenable.h b/third_party/android_system_core/include/utils/Flattenable.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Flattenable.h
rename to third_party/android_system_core/include/utils/Flattenable.h
diff --git a/phonelibs/android_system_core/include/utils/Functor.h b/third_party/android_system_core/include/utils/Functor.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Functor.h
rename to third_party/android_system_core/include/utils/Functor.h
diff --git a/phonelibs/android_system_core/include/utils/JenkinsHash.h b/third_party/android_system_core/include/utils/JenkinsHash.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/JenkinsHash.h
rename to third_party/android_system_core/include/utils/JenkinsHash.h
diff --git a/phonelibs/android_system_core/include/utils/KeyedVector.h b/third_party/android_system_core/include/utils/KeyedVector.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/KeyedVector.h
rename to third_party/android_system_core/include/utils/KeyedVector.h
diff --git a/phonelibs/android_system_core/include/utils/LinearTransform.h b/third_party/android_system_core/include/utils/LinearTransform.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/LinearTransform.h
rename to third_party/android_system_core/include/utils/LinearTransform.h
diff --git a/phonelibs/android_system_core/include/utils/List.h b/third_party/android_system_core/include/utils/List.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/List.h
rename to third_party/android_system_core/include/utils/List.h
diff --git a/phonelibs/android_system_core/include/utils/Log.h b/third_party/android_system_core/include/utils/Log.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Log.h
rename to third_party/android_system_core/include/utils/Log.h
diff --git a/phonelibs/android_system_core/include/utils/Looper.h b/third_party/android_system_core/include/utils/Looper.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Looper.h
rename to third_party/android_system_core/include/utils/Looper.h
diff --git a/phonelibs/android_system_core/include/utils/LruCache.h b/third_party/android_system_core/include/utils/LruCache.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/LruCache.h
rename to third_party/android_system_core/include/utils/LruCache.h
diff --git a/phonelibs/android_system_core/include/utils/Mutex.h b/third_party/android_system_core/include/utils/Mutex.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Mutex.h
rename to third_party/android_system_core/include/utils/Mutex.h
diff --git a/phonelibs/android_system_core/include/utils/NativeHandle.h b/third_party/android_system_core/include/utils/NativeHandle.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/NativeHandle.h
rename to third_party/android_system_core/include/utils/NativeHandle.h
diff --git a/phonelibs/android_system_core/include/utils/Printer.h b/third_party/android_system_core/include/utils/Printer.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Printer.h
rename to third_party/android_system_core/include/utils/Printer.h
diff --git a/phonelibs/android_system_core/include/utils/ProcessCallStack.h b/third_party/android_system_core/include/utils/ProcessCallStack.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/ProcessCallStack.h
rename to third_party/android_system_core/include/utils/ProcessCallStack.h
diff --git a/phonelibs/android_system_core/include/utils/PropertyMap.h b/third_party/android_system_core/include/utils/PropertyMap.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/PropertyMap.h
rename to third_party/android_system_core/include/utils/PropertyMap.h
diff --git a/phonelibs/android_system_core/include/utils/RWLock.h b/third_party/android_system_core/include/utils/RWLock.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/RWLock.h
rename to third_party/android_system_core/include/utils/RWLock.h
diff --git a/phonelibs/android_system_core/include/utils/RefBase.h b/third_party/android_system_core/include/utils/RefBase.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/RefBase.h
rename to third_party/android_system_core/include/utils/RefBase.h
diff --git a/phonelibs/android_system_core/include/utils/SharedBuffer.h b/third_party/android_system_core/include/utils/SharedBuffer.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/SharedBuffer.h
rename to third_party/android_system_core/include/utils/SharedBuffer.h
diff --git a/phonelibs/android_system_core/include/utils/Singleton.h b/third_party/android_system_core/include/utils/Singleton.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Singleton.h
rename to third_party/android_system_core/include/utils/Singleton.h
diff --git a/phonelibs/android_system_core/include/utils/SortedVector.h b/third_party/android_system_core/include/utils/SortedVector.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/SortedVector.h
rename to third_party/android_system_core/include/utils/SortedVector.h
diff --git a/phonelibs/android_system_core/include/utils/StopWatch.h b/third_party/android_system_core/include/utils/StopWatch.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/StopWatch.h
rename to third_party/android_system_core/include/utils/StopWatch.h
diff --git a/phonelibs/android_system_core/include/utils/String16.h b/third_party/android_system_core/include/utils/String16.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/String16.h
rename to third_party/android_system_core/include/utils/String16.h
diff --git a/phonelibs/android_system_core/include/utils/String8.h b/third_party/android_system_core/include/utils/String8.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/String8.h
rename to third_party/android_system_core/include/utils/String8.h
diff --git a/phonelibs/android_system_core/include/utils/StrongPointer.h b/third_party/android_system_core/include/utils/StrongPointer.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/StrongPointer.h
rename to third_party/android_system_core/include/utils/StrongPointer.h
diff --git a/phonelibs/android_system_core/include/utils/SystemClock.h b/third_party/android_system_core/include/utils/SystemClock.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/SystemClock.h
rename to third_party/android_system_core/include/utils/SystemClock.h
diff --git a/phonelibs/android_system_core/include/utils/Thread.h b/third_party/android_system_core/include/utils/Thread.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Thread.h
rename to third_party/android_system_core/include/utils/Thread.h
diff --git a/phonelibs/android_system_core/include/utils/ThreadDefs.h b/third_party/android_system_core/include/utils/ThreadDefs.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/ThreadDefs.h
rename to third_party/android_system_core/include/utils/ThreadDefs.h
diff --git a/phonelibs/android_system_core/include/utils/Timers.h b/third_party/android_system_core/include/utils/Timers.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Timers.h
rename to third_party/android_system_core/include/utils/Timers.h
diff --git a/phonelibs/android_system_core/include/utils/Tokenizer.h b/third_party/android_system_core/include/utils/Tokenizer.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Tokenizer.h
rename to third_party/android_system_core/include/utils/Tokenizer.h
diff --git a/phonelibs/android_system_core/include/utils/Trace.h b/third_party/android_system_core/include/utils/Trace.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Trace.h
rename to third_party/android_system_core/include/utils/Trace.h
diff --git a/phonelibs/android_system_core/include/utils/TypeHelpers.h b/third_party/android_system_core/include/utils/TypeHelpers.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/TypeHelpers.h
rename to third_party/android_system_core/include/utils/TypeHelpers.h
diff --git a/phonelibs/android_system_core/include/utils/Unicode.h b/third_party/android_system_core/include/utils/Unicode.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Unicode.h
rename to third_party/android_system_core/include/utils/Unicode.h
diff --git a/phonelibs/android_system_core/include/utils/Vector.h b/third_party/android_system_core/include/utils/Vector.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/Vector.h
rename to third_party/android_system_core/include/utils/Vector.h
diff --git a/phonelibs/android_system_core/include/utils/VectorImpl.h b/third_party/android_system_core/include/utils/VectorImpl.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/VectorImpl.h
rename to third_party/android_system_core/include/utils/VectorImpl.h
diff --git a/phonelibs/android_system_core/include/utils/ashmem.h b/third_party/android_system_core/include/utils/ashmem.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/ashmem.h
rename to third_party/android_system_core/include/utils/ashmem.h
diff --git a/phonelibs/android_system_core/include/utils/misc.h b/third_party/android_system_core/include/utils/misc.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/misc.h
rename to third_party/android_system_core/include/utils/misc.h
diff --git a/phonelibs/android_system_core/include/utils/threads.h b/third_party/android_system_core/include/utils/threads.h
similarity index 100%
rename from phonelibs/android_system_core/include/utils/threads.h
rename to third_party/android_system_core/include/utils/threads.h
diff --git a/phonelibs/bzip2/LICENSE b/third_party/bzip2/LICENSE
similarity index 100%
rename from phonelibs/bzip2/LICENSE
rename to third_party/bzip2/LICENSE
diff --git a/phonelibs/bzip2/build.txt b/third_party/bzip2/build.txt
similarity index 100%
rename from phonelibs/bzip2/build.txt
rename to third_party/bzip2/build.txt
diff --git a/phonelibs/bzip2/bzlib.h b/third_party/bzip2/bzlib.h
similarity index 100%
rename from phonelibs/bzip2/bzlib.h
rename to third_party/bzip2/bzlib.h
diff --git a/phonelibs/bzip2/libbz2.a b/third_party/bzip2/libbz2.a
similarity index 100%
rename from phonelibs/bzip2/libbz2.a
rename to third_party/bzip2/libbz2.a
diff --git a/phonelibs/json11/json11.cpp b/third_party/json11/json11.cpp
similarity index 100%
rename from phonelibs/json11/json11.cpp
rename to third_party/json11/json11.cpp
diff --git a/phonelibs/json11/json11.hpp b/third_party/json11/json11.hpp
similarity index 100%
rename from phonelibs/json11/json11.hpp
rename to third_party/json11/json11.hpp
diff --git a/phonelibs/kaitai/custom_decoder.h b/third_party/kaitai/custom_decoder.h
similarity index 100%
rename from phonelibs/kaitai/custom_decoder.h
rename to third_party/kaitai/custom_decoder.h
diff --git a/phonelibs/kaitai/exceptions.h b/third_party/kaitai/exceptions.h
similarity index 100%
rename from phonelibs/kaitai/exceptions.h
rename to third_party/kaitai/exceptions.h
diff --git a/phonelibs/kaitai/kaitaistream.cpp b/third_party/kaitai/kaitaistream.cpp
similarity index 100%
rename from phonelibs/kaitai/kaitaistream.cpp
rename to third_party/kaitai/kaitaistream.cpp
diff --git a/phonelibs/kaitai/kaitaistream.h b/third_party/kaitai/kaitaistream.h
similarity index 100%
rename from phonelibs/kaitai/kaitaistream.h
rename to third_party/kaitai/kaitaistream.h
diff --git a/phonelibs/kaitai/kaitaistruct.h b/third_party/kaitai/kaitaistruct.h
similarity index 100%
rename from phonelibs/kaitai/kaitaistruct.h
rename to third_party/kaitai/kaitaistruct.h
diff --git a/phonelibs/libgralloc/include/gralloc_priv.h b/third_party/libgralloc/include/gralloc_priv.h
similarity index 100%
rename from phonelibs/libgralloc/include/gralloc_priv.h
rename to third_party/libgralloc/include/gralloc_priv.h
diff --git a/phonelibs/libyuv/include/libyuv.h b/third_party/libyuv/include/libyuv.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv.h
rename to third_party/libyuv/include/libyuv.h
diff --git a/phonelibs/libyuv/include/libyuv/basic_types.h b/third_party/libyuv/include/libyuv/basic_types.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/basic_types.h
rename to third_party/libyuv/include/libyuv/basic_types.h
diff --git a/phonelibs/libyuv/include/libyuv/compare.h b/third_party/libyuv/include/libyuv/compare.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/compare.h
rename to third_party/libyuv/include/libyuv/compare.h
diff --git a/phonelibs/libyuv/include/libyuv/compare_row.h b/third_party/libyuv/include/libyuv/compare_row.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/compare_row.h
rename to third_party/libyuv/include/libyuv/compare_row.h
diff --git a/phonelibs/libyuv/include/libyuv/convert.h b/third_party/libyuv/include/libyuv/convert.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/convert.h
rename to third_party/libyuv/include/libyuv/convert.h
diff --git a/phonelibs/libyuv/include/libyuv/convert_argb.h b/third_party/libyuv/include/libyuv/convert_argb.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/convert_argb.h
rename to third_party/libyuv/include/libyuv/convert_argb.h
diff --git a/phonelibs/libyuv/include/libyuv/convert_from.h b/third_party/libyuv/include/libyuv/convert_from.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/convert_from.h
rename to third_party/libyuv/include/libyuv/convert_from.h
diff --git a/phonelibs/libyuv/include/libyuv/convert_from_argb.h b/third_party/libyuv/include/libyuv/convert_from_argb.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/convert_from_argb.h
rename to third_party/libyuv/include/libyuv/convert_from_argb.h
diff --git a/phonelibs/libyuv/include/libyuv/cpu_id.h b/third_party/libyuv/include/libyuv/cpu_id.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/cpu_id.h
rename to third_party/libyuv/include/libyuv/cpu_id.h
diff --git a/phonelibs/libyuv/include/libyuv/macros_msa.h b/third_party/libyuv/include/libyuv/macros_msa.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/macros_msa.h
rename to third_party/libyuv/include/libyuv/macros_msa.h
diff --git a/phonelibs/libyuv/include/libyuv/mjpeg_decoder.h b/third_party/libyuv/include/libyuv/mjpeg_decoder.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/mjpeg_decoder.h
rename to third_party/libyuv/include/libyuv/mjpeg_decoder.h
diff --git a/phonelibs/libyuv/include/libyuv/planar_functions.h b/third_party/libyuv/include/libyuv/planar_functions.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/planar_functions.h
rename to third_party/libyuv/include/libyuv/planar_functions.h
diff --git a/phonelibs/libyuv/include/libyuv/rotate.h b/third_party/libyuv/include/libyuv/rotate.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/rotate.h
rename to third_party/libyuv/include/libyuv/rotate.h
diff --git a/phonelibs/libyuv/include/libyuv/rotate_argb.h b/third_party/libyuv/include/libyuv/rotate_argb.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/rotate_argb.h
rename to third_party/libyuv/include/libyuv/rotate_argb.h
diff --git a/phonelibs/libyuv/include/libyuv/rotate_row.h b/third_party/libyuv/include/libyuv/rotate_row.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/rotate_row.h
rename to third_party/libyuv/include/libyuv/rotate_row.h
diff --git a/phonelibs/libyuv/include/libyuv/row.h b/third_party/libyuv/include/libyuv/row.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/row.h
rename to third_party/libyuv/include/libyuv/row.h
diff --git a/phonelibs/libyuv/include/libyuv/scale.h b/third_party/libyuv/include/libyuv/scale.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/scale.h
rename to third_party/libyuv/include/libyuv/scale.h
diff --git a/phonelibs/libyuv/include/libyuv/scale_argb.h b/third_party/libyuv/include/libyuv/scale_argb.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/scale_argb.h
rename to third_party/libyuv/include/libyuv/scale_argb.h
diff --git a/phonelibs/libyuv/include/libyuv/scale_row.h b/third_party/libyuv/include/libyuv/scale_row.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/scale_row.h
rename to third_party/libyuv/include/libyuv/scale_row.h
diff --git a/phonelibs/libyuv/include/libyuv/version.h b/third_party/libyuv/include/libyuv/version.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/version.h
rename to third_party/libyuv/include/libyuv/version.h
diff --git a/phonelibs/libyuv/include/libyuv/video_common.h b/third_party/libyuv/include/libyuv/video_common.h
similarity index 100%
rename from phonelibs/libyuv/include/libyuv/video_common.h
rename to third_party/libyuv/include/libyuv/video_common.h
diff --git a/phonelibs/libyuv/larch64/lib/libyuv.a b/third_party/libyuv/larch64/lib/libyuv.a
similarity index 100%
rename from phonelibs/libyuv/larch64/lib/libyuv.a
rename to third_party/libyuv/larch64/lib/libyuv.a
diff --git a/phonelibs/libyuv/lib/libyuv.a b/third_party/libyuv/lib/libyuv.a
similarity index 100%
rename from phonelibs/libyuv/lib/libyuv.a
rename to third_party/libyuv/lib/libyuv.a
diff --git a/phonelibs/linux/include/linux/ion.h b/third_party/linux/include/linux/ion.h
similarity index 100%
rename from phonelibs/linux/include/linux/ion.h
rename to third_party/linux/include/linux/ion.h
diff --git a/phonelibs/linux/include/msm_ion.h b/third_party/linux/include/msm_ion.h
similarity index 100%
rename from phonelibs/linux/include/msm_ion.h
rename to third_party/linux/include/msm_ion.h
diff --git a/phonelibs/mapbox-gl-native-qt/include/QMapbox b/third_party/mapbox-gl-native-qt/include/QMapbox
similarity index 100%
rename from phonelibs/mapbox-gl-native-qt/include/QMapbox
rename to third_party/mapbox-gl-native-qt/include/QMapbox
diff --git a/phonelibs/mapbox-gl-native-qt/include/QMapboxGL b/third_party/mapbox-gl-native-qt/include/QMapboxGL
similarity index 100%
rename from phonelibs/mapbox-gl-native-qt/include/QMapboxGL
rename to third_party/mapbox-gl-native-qt/include/QMapboxGL
diff --git a/phonelibs/mapbox-gl-native-qt/include/qmapbox.hpp b/third_party/mapbox-gl-native-qt/include/qmapbox.hpp
similarity index 100%
rename from phonelibs/mapbox-gl-native-qt/include/qmapbox.hpp
rename to third_party/mapbox-gl-native-qt/include/qmapbox.hpp
diff --git a/phonelibs/mapbox-gl-native-qt/include/qmapboxgl.hpp b/third_party/mapbox-gl-native-qt/include/qmapboxgl.hpp
similarity index 100%
rename from phonelibs/mapbox-gl-native-qt/include/qmapboxgl.hpp
rename to third_party/mapbox-gl-native-qt/include/qmapboxgl.hpp
diff --git a/phonelibs/nanovg/fontstash.h b/third_party/nanovg/fontstash.h
similarity index 100%
rename from phonelibs/nanovg/fontstash.h
rename to third_party/nanovg/fontstash.h
diff --git a/phonelibs/nanovg/nanovg.c b/third_party/nanovg/nanovg.c
similarity index 100%
rename from phonelibs/nanovg/nanovg.c
rename to third_party/nanovg/nanovg.c
diff --git a/phonelibs/nanovg/nanovg.h b/third_party/nanovg/nanovg.h
similarity index 100%
rename from phonelibs/nanovg/nanovg.h
rename to third_party/nanovg/nanovg.h
diff --git a/phonelibs/nanovg/nanovg_gl.h b/third_party/nanovg/nanovg_gl.h
similarity index 100%
rename from phonelibs/nanovg/nanovg_gl.h
rename to third_party/nanovg/nanovg_gl.h
diff --git a/phonelibs/nanovg/nanovg_gl_utils.h b/third_party/nanovg/nanovg_gl_utils.h
similarity index 100%
rename from phonelibs/nanovg/nanovg_gl_utils.h
rename to third_party/nanovg/nanovg_gl_utils.h
diff --git a/phonelibs/nanovg/stb_image.h b/third_party/nanovg/stb_image.h
similarity index 100%
rename from phonelibs/nanovg/stb_image.h
rename to third_party/nanovg/stb_image.h
diff --git a/phonelibs/nanovg/stb_truetype.h b/third_party/nanovg/stb_truetype.h
similarity index 100%
rename from phonelibs/nanovg/stb_truetype.h
rename to third_party/nanovg/stb_truetype.h
diff --git a/phonelibs/opencl/include/CL/cl.h b/third_party/opencl/include/CL/cl.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl.h
rename to third_party/opencl/include/CL/cl.h
diff --git a/phonelibs/opencl/include/CL/cl_d3d10.h b/third_party/opencl/include/CL/cl_d3d10.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_d3d10.h
rename to third_party/opencl/include/CL/cl_d3d10.h
diff --git a/phonelibs/opencl/include/CL/cl_d3d11.h b/third_party/opencl/include/CL/cl_d3d11.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_d3d11.h
rename to third_party/opencl/include/CL/cl_d3d11.h
diff --git a/phonelibs/opencl/include/CL/cl_dx9_media_sharing.h b/third_party/opencl/include/CL/cl_dx9_media_sharing.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_dx9_media_sharing.h
rename to third_party/opencl/include/CL/cl_dx9_media_sharing.h
diff --git a/phonelibs/opencl/include/CL/cl_egl.h b/third_party/opencl/include/CL/cl_egl.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_egl.h
rename to third_party/opencl/include/CL/cl_egl.h
diff --git a/phonelibs/opencl/include/CL/cl_ext.h b/third_party/opencl/include/CL/cl_ext.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_ext.h
rename to third_party/opencl/include/CL/cl_ext.h
diff --git a/phonelibs/opencl/include/CL/cl_ext_qcom.h b/third_party/opencl/include/CL/cl_ext_qcom.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_ext_qcom.h
rename to third_party/opencl/include/CL/cl_ext_qcom.h
diff --git a/phonelibs/opencl/include/CL/cl_gl.h b/third_party/opencl/include/CL/cl_gl.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_gl.h
rename to third_party/opencl/include/CL/cl_gl.h
diff --git a/phonelibs/opencl/include/CL/cl_gl_ext.h b/third_party/opencl/include/CL/cl_gl_ext.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_gl_ext.h
rename to third_party/opencl/include/CL/cl_gl_ext.h
diff --git a/phonelibs/opencl/include/CL/cl_platform.h b/third_party/opencl/include/CL/cl_platform.h
similarity index 100%
rename from phonelibs/opencl/include/CL/cl_platform.h
rename to third_party/opencl/include/CL/cl_platform.h
diff --git a/phonelibs/opencl/include/CL/opencl.h b/third_party/opencl/include/CL/opencl.h
similarity index 100%
rename from phonelibs/opencl/include/CL/opencl.h
rename to third_party/opencl/include/CL/opencl.h
diff --git a/phonelibs/openmax/include/OMX_Audio.h b/third_party/openmax/include/OMX_Audio.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Audio.h
rename to third_party/openmax/include/OMX_Audio.h
diff --git a/phonelibs/openmax/include/OMX_Component.h b/third_party/openmax/include/OMX_Component.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Component.h
rename to third_party/openmax/include/OMX_Component.h
diff --git a/phonelibs/openmax/include/OMX_ContentPipe.h b/third_party/openmax/include/OMX_ContentPipe.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_ContentPipe.h
rename to third_party/openmax/include/OMX_ContentPipe.h
diff --git a/phonelibs/openmax/include/OMX_Core.h b/third_party/openmax/include/OMX_Core.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Core.h
rename to third_party/openmax/include/OMX_Core.h
diff --git a/phonelibs/openmax/include/OMX_CoreExt.h b/third_party/openmax/include/OMX_CoreExt.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_CoreExt.h
rename to third_party/openmax/include/OMX_CoreExt.h
diff --git a/phonelibs/openmax/include/OMX_IVCommon.h b/third_party/openmax/include/OMX_IVCommon.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_IVCommon.h
rename to third_party/openmax/include/OMX_IVCommon.h
diff --git a/phonelibs/openmax/include/OMX_Image.h b/third_party/openmax/include/OMX_Image.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Image.h
rename to third_party/openmax/include/OMX_Image.h
diff --git a/phonelibs/openmax/include/OMX_Index.h b/third_party/openmax/include/OMX_Index.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Index.h
rename to third_party/openmax/include/OMX_Index.h
diff --git a/phonelibs/openmax/include/OMX_IndexExt.h b/third_party/openmax/include/OMX_IndexExt.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_IndexExt.h
rename to third_party/openmax/include/OMX_IndexExt.h
diff --git a/phonelibs/openmax/include/OMX_Other.h b/third_party/openmax/include/OMX_Other.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Other.h
rename to third_party/openmax/include/OMX_Other.h
diff --git a/phonelibs/openmax/include/OMX_QCOMExtns.h b/third_party/openmax/include/OMX_QCOMExtns.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_QCOMExtns.h
rename to third_party/openmax/include/OMX_QCOMExtns.h
diff --git a/phonelibs/openmax/include/OMX_Skype_VideoExtensions.h b/third_party/openmax/include/OMX_Skype_VideoExtensions.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Skype_VideoExtensions.h
rename to third_party/openmax/include/OMX_Skype_VideoExtensions.h
diff --git a/phonelibs/openmax/include/OMX_Types.h b/third_party/openmax/include/OMX_Types.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Types.h
rename to third_party/openmax/include/OMX_Types.h
diff --git a/phonelibs/openmax/include/OMX_Video.h b/third_party/openmax/include/OMX_Video.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_Video.h
rename to third_party/openmax/include/OMX_Video.h
diff --git a/phonelibs/openmax/include/OMX_VideoExt.h b/third_party/openmax/include/OMX_VideoExt.h
similarity index 100%
rename from phonelibs/openmax/include/OMX_VideoExt.h
rename to third_party/openmax/include/OMX_VideoExt.h
diff --git a/phonelibs/qrcode/QrCode.cc b/third_party/qrcode/QrCode.cc
similarity index 100%
rename from phonelibs/qrcode/QrCode.cc
rename to third_party/qrcode/QrCode.cc
diff --git a/phonelibs/qrcode/QrCode.hpp b/third_party/qrcode/QrCode.hpp
similarity index 100%
rename from phonelibs/qrcode/QrCode.hpp
rename to third_party/qrcode/QrCode.hpp
diff --git a/phonelibs/snpe/aarch64 b/third_party/snpe/aarch64
similarity index 100%
rename from phonelibs/snpe/aarch64
rename to third_party/snpe/aarch64
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so b/third_party/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so
rename to third_party/snpe/aarch64-android-clang6.0/libPlatformValidatorShared.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so b/third_party/snpe/aarch64-android-clang6.0/libSNPE.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libSNPE.so
rename to third_party/snpe/aarch64-android-clang6.0/libSNPE.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libSNPE_G.so b/third_party/snpe/aarch64-android-clang6.0/libSNPE_G.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libSNPE_G.so
rename to third_party/snpe/aarch64-android-clang6.0/libSNPE_G.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libc++_shared.so b/third_party/snpe/aarch64-android-clang6.0/libc++_shared.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libc++_shared.so
rename to third_party/snpe/aarch64-android-clang6.0/libc++_shared.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libcalculator.so b/third_party/snpe/aarch64-android-clang6.0/libcalculator.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libcalculator.so
rename to third_party/snpe/aarch64-android-clang6.0/libcalculator.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libcalculator_domains.so b/third_party/snpe/aarch64-android-clang6.0/libcalculator_domains.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libcalculator_domains.so
rename to third_party/snpe/aarch64-android-clang6.0/libcalculator_domains.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libhta.so b/third_party/snpe/aarch64-android-clang6.0/libhta.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libhta.so
rename to third_party/snpe/aarch64-android-clang6.0/libhta.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so b/third_party/snpe/aarch64-android-clang6.0/libsnpe_adsp.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsnpe_adsp.so
rename to third_party/snpe/aarch64-android-clang6.0/libsnpe_adsp.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so b/third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so
rename to third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so b/third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so
rename to third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_system.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so b/third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so
rename to third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so b/third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so
rename to third_party/snpe/aarch64-android-clang6.0/libsnpe_dsp_domains_v2_system.so
diff --git a/phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so b/third_party/snpe/aarch64-android-clang6.0/libsymphony-cpu.so
similarity index 100%
rename from phonelibs/snpe/aarch64-android-clang6.0/libsymphony-cpu.so
rename to third_party/snpe/aarch64-android-clang6.0/libsymphony-cpu.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so b/third_party/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libPlatformValidatorShared.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so b/third_party/snpe/aarch64-linux-gcc4.9/libSNPE.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libSNPE.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libSNPE.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1 b/third_party/snpe/aarch64-linux-gcc4.9/libatomic.so.1
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libatomic.so.1
rename to third_party/snpe/aarch64-linux-gcc4.9/libatomic.so.1
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so b/third_party/snpe/aarch64-linux-gcc4.9/libcalculator.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libcalculator.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so b/third_party/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libcalculator_domains.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so b/third_party/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libsnpe_adsp.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so b/third_party/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libsnpe_dsp_domains_v2.so
diff --git a/phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so b/third_party/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so
similarity index 100%
rename from phonelibs/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so
rename to third_party/snpe/aarch64-linux-gcc4.9/libsymphony-cpu.so
diff --git a/phonelibs/snpe/dsp/libcalculator_domains_skel.so b/third_party/snpe/dsp/libcalculator_domains_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libcalculator_domains_skel.so
rename to third_party/snpe/dsp/libcalculator_domains_skel.so
diff --git a/phonelibs/snpe/dsp/libcalculator_skel.so b/third_party/snpe/dsp/libcalculator_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libcalculator_skel.so
rename to third_party/snpe/dsp/libcalculator_skel.so
diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_domains_skel.so b/third_party/snpe/dsp/libsnpe_dsp_domains_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libsnpe_dsp_domains_skel.so
rename to third_party/snpe/dsp/libsnpe_dsp_domains_skel.so
diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_skel.so b/third_party/snpe/dsp/libsnpe_dsp_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libsnpe_dsp_skel.so
rename to third_party/snpe/dsp/libsnpe_dsp_skel.so
diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so b/third_party/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so
rename to third_party/snpe/dsp/libsnpe_dsp_v65_domains_v2_skel.so
diff --git a/phonelibs/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so b/third_party/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so
similarity index 100%
rename from phonelibs/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so
rename to third_party/snpe/dsp/libsnpe_dsp_v66_domains_v2_skel.so
diff --git a/phonelibs/snpe/include/DiagLog/IDiagLog.hpp b/third_party/snpe/include/DiagLog/IDiagLog.hpp
similarity index 100%
rename from phonelibs/snpe/include/DiagLog/IDiagLog.hpp
rename to third_party/snpe/include/DiagLog/IDiagLog.hpp
diff --git a/phonelibs/snpe/include/DiagLog/Options.hpp b/third_party/snpe/include/DiagLog/Options.hpp
similarity index 100%
rename from phonelibs/snpe/include/DiagLog/Options.hpp
rename to third_party/snpe/include/DiagLog/Options.hpp
diff --git a/phonelibs/snpe/include/DlContainer/IDlContainer.hpp b/third_party/snpe/include/DlContainer/IDlContainer.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlContainer/IDlContainer.hpp
rename to third_party/snpe/include/DlContainer/IDlContainer.hpp
diff --git a/phonelibs/snpe/include/DlSystem/DlEnums.hpp b/third_party/snpe/include/DlSystem/DlEnums.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/DlEnums.hpp
rename to third_party/snpe/include/DlSystem/DlEnums.hpp
diff --git a/phonelibs/snpe/include/DlSystem/DlError.hpp b/third_party/snpe/include/DlSystem/DlError.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/DlError.hpp
rename to third_party/snpe/include/DlSystem/DlError.hpp
diff --git a/phonelibs/snpe/include/DlSystem/DlOptional.hpp b/third_party/snpe/include/DlSystem/DlOptional.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/DlOptional.hpp
rename to third_party/snpe/include/DlSystem/DlOptional.hpp
diff --git a/phonelibs/snpe/include/DlSystem/DlVersion.hpp b/third_party/snpe/include/DlSystem/DlVersion.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/DlVersion.hpp
rename to third_party/snpe/include/DlSystem/DlVersion.hpp
diff --git a/phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp b/third_party/snpe/include/DlSystem/IBufferAttributes.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/IBufferAttributes.hpp
rename to third_party/snpe/include/DlSystem/IBufferAttributes.hpp
diff --git a/phonelibs/snpe/include/DlSystem/ITensor.hpp b/third_party/snpe/include/DlSystem/ITensor.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/ITensor.hpp
rename to third_party/snpe/include/DlSystem/ITensor.hpp
diff --git a/phonelibs/snpe/include/DlSystem/ITensorFactory.hpp b/third_party/snpe/include/DlSystem/ITensorFactory.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/ITensorFactory.hpp
rename to third_party/snpe/include/DlSystem/ITensorFactory.hpp
diff --git a/phonelibs/snpe/include/DlSystem/ITensorItr.hpp b/third_party/snpe/include/DlSystem/ITensorItr.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/ITensorItr.hpp
rename to third_party/snpe/include/DlSystem/ITensorItr.hpp
diff --git a/phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp b/third_party/snpe/include/DlSystem/ITensorItrImpl.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/ITensorItrImpl.hpp
rename to third_party/snpe/include/DlSystem/ITensorItrImpl.hpp
diff --git a/phonelibs/snpe/include/DlSystem/IUDL.hpp b/third_party/snpe/include/DlSystem/IUDL.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/IUDL.hpp
rename to third_party/snpe/include/DlSystem/IUDL.hpp
diff --git a/phonelibs/snpe/include/DlSystem/IUserBuffer.hpp b/third_party/snpe/include/DlSystem/IUserBuffer.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/IUserBuffer.hpp
rename to third_party/snpe/include/DlSystem/IUserBuffer.hpp
diff --git a/phonelibs/snpe/include/DlSystem/IUserBufferFactory.hpp b/third_party/snpe/include/DlSystem/IUserBufferFactory.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/IUserBufferFactory.hpp
rename to third_party/snpe/include/DlSystem/IUserBufferFactory.hpp
diff --git a/phonelibs/snpe/include/DlSystem/PlatformConfig.hpp b/third_party/snpe/include/DlSystem/PlatformConfig.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/PlatformConfig.hpp
rename to third_party/snpe/include/DlSystem/PlatformConfig.hpp
diff --git a/phonelibs/snpe/include/DlSystem/RuntimeList.hpp b/third_party/snpe/include/DlSystem/RuntimeList.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/RuntimeList.hpp
rename to third_party/snpe/include/DlSystem/RuntimeList.hpp
diff --git a/phonelibs/snpe/include/DlSystem/String.hpp b/third_party/snpe/include/DlSystem/String.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/String.hpp
rename to third_party/snpe/include/DlSystem/String.hpp
diff --git a/phonelibs/snpe/include/DlSystem/StringList.hpp b/third_party/snpe/include/DlSystem/StringList.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/StringList.hpp
rename to third_party/snpe/include/DlSystem/StringList.hpp
diff --git a/phonelibs/snpe/include/DlSystem/TensorMap.hpp b/third_party/snpe/include/DlSystem/TensorMap.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/TensorMap.hpp
rename to third_party/snpe/include/DlSystem/TensorMap.hpp
diff --git a/phonelibs/snpe/include/DlSystem/TensorShape.hpp b/third_party/snpe/include/DlSystem/TensorShape.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/TensorShape.hpp
rename to third_party/snpe/include/DlSystem/TensorShape.hpp
diff --git a/phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp b/third_party/snpe/include/DlSystem/TensorShapeMap.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/TensorShapeMap.hpp
rename to third_party/snpe/include/DlSystem/TensorShapeMap.hpp
diff --git a/phonelibs/snpe/include/DlSystem/UDLContext.hpp b/third_party/snpe/include/DlSystem/UDLContext.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/UDLContext.hpp
rename to third_party/snpe/include/DlSystem/UDLContext.hpp
diff --git a/phonelibs/snpe/include/DlSystem/UDLFunc.hpp b/third_party/snpe/include/DlSystem/UDLFunc.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/UDLFunc.hpp
rename to third_party/snpe/include/DlSystem/UDLFunc.hpp
diff --git a/phonelibs/snpe/include/DlSystem/UserBufferMap.hpp b/third_party/snpe/include/DlSystem/UserBufferMap.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/UserBufferMap.hpp
rename to third_party/snpe/include/DlSystem/UserBufferMap.hpp
diff --git a/phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp b/third_party/snpe/include/DlSystem/ZdlExportDefine.hpp
similarity index 100%
rename from phonelibs/snpe/include/DlSystem/ZdlExportDefine.hpp
rename to third_party/snpe/include/DlSystem/ZdlExportDefine.hpp
diff --git a/phonelibs/snpe/include/PlatformValidator/PlatformValidator.hpp b/third_party/snpe/include/PlatformValidator/PlatformValidator.hpp
similarity index 100%
rename from phonelibs/snpe/include/PlatformValidator/PlatformValidator.hpp
rename to third_party/snpe/include/PlatformValidator/PlatformValidator.hpp
diff --git a/phonelibs/snpe/include/SNPE/ApplicationBufferMap.hpp b/third_party/snpe/include/SNPE/ApplicationBufferMap.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/ApplicationBufferMap.hpp
rename to third_party/snpe/include/SNPE/ApplicationBufferMap.hpp
diff --git a/phonelibs/snpe/include/SNPE/PSNPE.hpp b/third_party/snpe/include/SNPE/PSNPE.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/PSNPE.hpp
rename to third_party/snpe/include/SNPE/PSNPE.hpp
diff --git a/phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp b/third_party/snpe/include/SNPE/RuntimeConfigList.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/RuntimeConfigList.hpp
rename to third_party/snpe/include/SNPE/RuntimeConfigList.hpp
diff --git a/phonelibs/snpe/include/SNPE/SNPE.hpp b/third_party/snpe/include/SNPE/SNPE.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/SNPE.hpp
rename to third_party/snpe/include/SNPE/SNPE.hpp
diff --git a/phonelibs/snpe/include/SNPE/SNPEBuilder.hpp b/third_party/snpe/include/SNPE/SNPEBuilder.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/SNPEBuilder.hpp
rename to third_party/snpe/include/SNPE/SNPEBuilder.hpp
diff --git a/phonelibs/snpe/include/SNPE/SNPEFactory.hpp b/third_party/snpe/include/SNPE/SNPEFactory.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/SNPEFactory.hpp
rename to third_party/snpe/include/SNPE/SNPEFactory.hpp
diff --git a/phonelibs/snpe/include/SNPE/UserBufferList.hpp b/third_party/snpe/include/SNPE/UserBufferList.hpp
similarity index 100%
rename from phonelibs/snpe/include/SNPE/UserBufferList.hpp
rename to third_party/snpe/include/SNPE/UserBufferList.hpp
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoBase.h b/third_party/snpe/include/SnpeUdo/UdoBase.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoBase.h
rename to third_party/snpe/include/SnpeUdo/UdoBase.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImpl.h b/third_party/snpe/include/SnpeUdo/UdoImpl.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoImpl.h
rename to third_party/snpe/include/SnpeUdo/UdoImpl.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplCpu.h b/third_party/snpe/include/SnpeUdo/UdoImplCpu.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoImplCpu.h
rename to third_party/snpe/include/SnpeUdo/UdoImplCpu.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplDsp.h b/third_party/snpe/include/SnpeUdo/UdoImplDsp.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoImplDsp.h
rename to third_party/snpe/include/SnpeUdo/UdoImplDsp.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoImplGpu.h b/third_party/snpe/include/SnpeUdo/UdoImplGpu.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoImplGpu.h
rename to third_party/snpe/include/SnpeUdo/UdoImplGpu.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoReg.h b/third_party/snpe/include/SnpeUdo/UdoReg.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoReg.h
rename to third_party/snpe/include/SnpeUdo/UdoReg.h
diff --git a/phonelibs/snpe/include/SnpeUdo/UdoShared.h b/third_party/snpe/include/SnpeUdo/UdoShared.h
similarity index 100%
rename from phonelibs/snpe/include/SnpeUdo/UdoShared.h
rename to third_party/snpe/include/SnpeUdo/UdoShared.h
diff --git a/phonelibs/snpe/larch64 b/third_party/snpe/larch64
similarity index 100%
rename from phonelibs/snpe/larch64
rename to third_party/snpe/larch64
diff --git a/phonelibs/zlib/build.txt b/third_party/zlib/build.txt
similarity index 100%
rename from phonelibs/zlib/build.txt
rename to third_party/zlib/build.txt
diff --git a/phonelibs/zlib/lib/libz.a b/third_party/zlib/lib/libz.a
similarity index 100%
rename from phonelibs/zlib/lib/libz.a
rename to third_party/zlib/lib/libz.a
diff --git a/tools/lib/README.md b/tools/lib/README.md
new file mode 100644
index 000000000..0e786504f
--- /dev/null
+++ b/tools/lib/README.md
@@ -0,0 +1,35 @@
+# tools libs
+
+## [route.py](route.py) & [logreader.py](logreader.py)
+
+Route is a class for conviently accessing all the [logs](/selfdrive/loggerd/) from your routes. The LogReader class reads the non-video logs, i.e. rlog.bz2 and qlog.bz2. There's also a matching FrameReader class for reading the videos.
+
+```python
+from tools.lib.route import Route
+from tools.lib.logreader import LogReader
+
+r = Route("4cf7a6ad03080c90|2021-09-29--13-46-36")
+
+# get a list of paths for the route's rlog files
+print(r.log_paths())
+
+# and road camera (fcamera.hevc) files
+print(r.camera_paths())
+
+# setup a LogReader to read the route's first rlog
+lr = LogReader(r.log_paths()[0])
+
+# print out all the messages in the log
+import codecs
+codecs.register_error("strict", codecs.backslashreplace_errors)
+for msg in lr:
+  print(msg)
+
+# setup a LogReader for the route's second qlog
+lr = LogReader(r.log_paths()[1])
+
+# print all the steering angles values from the log
+for msg in lr:
+  if msg.which() == "carState":
+    print(msg.carState.steeringAngleDeg)
+```
diff --git a/selfdrive/controls/lib/lead_mpc_lib/__init__.py b/tools/lib/__init__.py
similarity index 100%
rename from selfdrive/controls/lib/lead_mpc_lib/__init__.py
rename to tools/lib/__init__.py
diff --git a/tools/lib/api.py b/tools/lib/api.py
new file mode 100644
index 000000000..6ff9242f2
--- /dev/null
+++ b/tools/lib/api.py
@@ -0,0 +1,34 @@
+import os
+import requests
+API_HOST = os.getenv('API_HOST', 'https://api.commadotai.com')
+
+class CommaApi():
+  def __init__(self, token=None):
+    self.session = requests.Session()
+    self.session.headers['User-agent'] = 'OpenpilotTools'
+    if token:
+      self.session.headers['Authorization'] = 'JWT ' + token
+
+  def request(self, method, endpoint, **kwargs):
+    resp = self.session.request(method, API_HOST + '/' + endpoint, **kwargs)
+    resp_json = resp.json()
+    if isinstance(resp_json, dict) and resp_json.get('error'):
+      if resp.status_code in [401, 403]:
+        raise UnauthorizedError('Unauthorized. Authenticate with tools/lib/auth.py')
+
+      e = APIError(str(resp.status_code) + ":" + resp_json.get('description', str(resp_json['error'])))
+      e.status_code = resp.status_code
+      raise e
+    return resp_json
+
+  def get(self, endpoint, **kwargs):
+    return self.request('GET', endpoint, **kwargs)
+
+  def post(self, endpoint, **kwargs):
+    return self.request('POST', endpoint, **kwargs)
+
+class APIError(Exception):
+  pass
+
+class UnauthorizedError(Exception):
+  pass
diff --git a/tools/lib/auth.py b/tools/lib/auth.py
new file mode 100755
index 000000000..ce15be98e
--- /dev/null
+++ b/tools/lib/auth.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+import sys
+import webbrowser
+from http.server import HTTPServer, BaseHTTPRequestHandler
+from urllib.parse import urlencode, parse_qs
+from tools.lib.api import CommaApi, APIError
+from tools.lib.auth_config import set_token
+from typing import Dict, Any
+
+PORT = 3000
+
+class ClientRedirectServer(HTTPServer):
+  query_params: Dict[str, Any] = {}
+
+class ClientRedirectHandler(BaseHTTPRequestHandler):
+  def do_GET(self):
+    if not self.path.startswith('/auth/g/redirect'):
+      self.send_response(204)
+      return
+
+    query = self.path.split('?', 1)[-1]
+    query = parse_qs(query, keep_blank_values=True)
+    self.server.query_params = query
+
+    self.send_response(200)
+    self.send_header('Content-type', 'text/plain')
+    self.end_headers()
+    self.wfile.write(b'Return to the CLI to continue')
+
+  def log_message(self, format, *args):  # pylint: disable=redefined-builtin
+    pass  # this prevent http server from dumping messages to stdout
+
+def auth_redirect_link():
+  redirect_uri = f'http://localhost:{PORT}/auth/g/redirect'
+  params = {
+    'type': 'web_server',
+    'client_id': '45471411055-ornt4svd2miog6dnopve7qtmh5mnu6id.apps.googleusercontent.com',
+    'redirect_uri': redirect_uri,
+    'response_type': 'code',
+    'scope': 'https://www.googleapis.com/auth/userinfo.email',
+    'prompt': 'select_account',
+  }
+
+  return (redirect_uri, 'https://accounts.google.com/o/oauth2/auth?' + urlencode(params))
+
+def login():
+  redirect_uri, oauth_uri = auth_redirect_link()
+
+  web_server = ClientRedirectServer(('localhost', PORT), ClientRedirectHandler)
+  print(f'To sign in, use your browser and navigate to {oauth_uri}')
+  webbrowser.open(oauth_uri, new=2)
+
+  while True:
+    web_server.handle_request()
+    if 'code' in web_server.query_params:
+      code = web_server.query_params['code']
+      break
+    elif 'error' in web_server.query_params:
+      print('Authentication Error: "%s". Description: "%s" ' % (
+        web_server.query_params['error'],
+        web_server.query_params.get('error_description')), file=sys.stderr)
+      break
+
+  try:
+    auth_resp = CommaApi().post('v2/auth/', data={'code': code, 'redirect_uri': redirect_uri})
+    set_token(auth_resp['access_token'])
+    print('Authenticated')
+  except APIError as e:
+    print(f'Authentication Error: {e}', file=sys.stderr)
+
+if __name__ == '__main__':
+  login()
diff --git a/tools/lib/auth_config.py b/tools/lib/auth_config.py
new file mode 100644
index 000000000..1699d94e5
--- /dev/null
+++ b/tools/lib/auth_config.py
@@ -0,0 +1,34 @@
+import json
+import os
+from common.file_helpers import mkdirs_exists_ok
+from selfdrive.hardware import PC
+
+
+class MissingAuthConfigError(Exception):
+  pass
+
+
+if PC:
+  CONFIG_DIR = os.path.expanduser('~/.comma')
+else:
+  CONFIG_DIR = "/tmp/.comma"
+
+mkdirs_exists_ok(CONFIG_DIR)
+
+
+def get_token():
+  try:
+    with open(os.path.join(CONFIG_DIR, 'auth.json')) as f:
+      auth = json.load(f)
+      return auth['access_token']
+  except Exception:
+    return None
+
+
+def set_token(token):
+  with open(os.path.join(CONFIG_DIR, 'auth.json'), 'w') as f:
+    json.dump({'access_token': token}, f)
+
+
+def clear_token():
+  os.unlink(os.path.join(CONFIG_DIR, 'auth.json'))
diff --git a/tools/lib/cache.py b/tools/lib/cache.py
new file mode 100644
index 000000000..82b229873
--- /dev/null
+++ b/tools/lib/cache.py
@@ -0,0 +1,15 @@
+import os
+import urllib.parse
+from common.file_helpers import mkdirs_exists_ok
+
+DEFAULT_CACHE_DIR = os.path.expanduser("~/.commacache")
+
+def cache_path_for_file_path(fn, cache_prefix=None):
+  dir_ = os.path.join(DEFAULT_CACHE_DIR, "local")
+  mkdirs_exists_ok(dir_)
+  fn_parsed = urllib.parse.urlparse(fn)
+  if fn_parsed.scheme == '':
+    cache_fn = os.path.abspath(fn).replace("/", "_")
+  else:
+    cache_fn = f'{fn_parsed.hostname}_{fn_parsed.path.replace("/", "_")}'
+  return os.path.join(dir_, cache_fn)
diff --git a/tools/lib/exceptions.py b/tools/lib/exceptions.py
new file mode 100644
index 000000000..41de2e864
--- /dev/null
+++ b/tools/lib/exceptions.py
@@ -0,0 +1,2 @@
+class DataUnreadableError(Exception):
+  pass
diff --git a/tools/lib/filereader.py b/tools/lib/filereader.py
new file mode 100644
index 000000000..3d4c46b22
--- /dev/null
+++ b/tools/lib/filereader.py
@@ -0,0 +1,6 @@
+from tools.lib.url_file import URLFile
+
+def FileReader(fn, debug=False):
+  if fn.startswith("http://") or fn.startswith("https://"):
+    return URLFile(fn, debug=debug)
+  return open(fn, "rb")
diff --git a/tools/lib/framereader.py b/tools/lib/framereader.py
new file mode 100644
index 000000000..00e28c9df
--- /dev/null
+++ b/tools/lib/framereader.py
@@ -0,0 +1,586 @@
+# pylint: skip-file
+import json
+import os
+import pickle
+import struct
+import subprocess
+import tempfile
+import threading
+from functools import wraps
+
+import numpy as np
+from aenum import Enum
+from lru import LRU
+
+import _io
+from tools.lib.cache import cache_path_for_file_path
+from tools.lib.exceptions import DataUnreadableError
+from common.file_helpers import atomic_write_in_dir
+
+try:
+  from xx.chffr.lib.filereader import FileReader
+except ImportError:
+  from tools.lib.filereader import FileReader
+
+HEVC_SLICE_B = 0
+HEVC_SLICE_P = 1
+HEVC_SLICE_I = 2
+
+
+class GOPReader:
+  def get_gop(self, num):
+    # returns (start_frame_num, num_frames, frames_to_skip, gop_data)
+    raise NotImplementedError
+
+
+class DoNothingContextManager:
+  def __enter__(self):
+    return self
+
+  def __exit__(self, *x):
+    pass
+
+
+class FrameType(Enum):
+  raw = 1
+  h265_stream = 2
+
+
+def fingerprint_video(fn):
+  with FileReader(fn) as f:
+    header = f.read(4)
+  if len(header) == 0:
+    raise DataUnreadableError("%s is empty" % fn)
+  elif header == b"\x00\xc0\x12\x00":
+    return FrameType.raw
+  elif header == b"\x00\x00\x00\x01":
+    if 'hevc' in fn:
+      return FrameType.h265_stream
+    else:
+      raise NotImplementedError(fn)
+  else:
+    raise NotImplementedError(fn)
+
+
+def ffprobe(fn, fmt=None):
+  cmd = ["ffprobe",
+         "-v", "quiet",
+         "-print_format", "json",
+         "-show_format", "-show_streams"]
+  if fmt:
+    cmd += ["-f", fmt]
+  cmd += [fn]
+
+  try:
+    ffprobe_output = subprocess.check_output(cmd)
+  except subprocess.CalledProcessError:
+    raise DataUnreadableError(fn)
+
+  return json.loads(ffprobe_output)
+
+
+def vidindex(fn, typ):
+  vidindex_dir = os.path.join(os.path.dirname(os.path.realpath(__file__)), "vidindex")
+  vidindex = os.path.join(vidindex_dir, "vidindex")
+
+  subprocess.check_call(["make"], cwd=vidindex_dir, stdout=open("/dev/null", "w"))
+
+  with tempfile.NamedTemporaryFile() as prefix_f, \
+       tempfile.NamedTemporaryFile() as index_f:
+    try:
+      subprocess.check_call([vidindex, typ, fn, prefix_f.name, index_f.name])
+    except subprocess.CalledProcessError:
+      raise DataUnreadableError("vidindex failed on file %s" % fn)
+    with open(index_f.name, "rb") as f:
+      index = f.read()
+    with open(prefix_f.name, "rb") as f:
+      prefix = f.read()
+
+  index = np.frombuffer(index, np.uint32).reshape(-1, 2)
+
+  assert index[-1, 0] == 0xFFFFFFFF
+  assert index[-1, 1] == os.path.getsize(fn)
+
+  return index, prefix
+
+
+def cache_fn(func):
+  @wraps(func)
+  def cache_inner(fn, *args, **kwargs):
+    if kwargs.pop('no_cache', None):
+      cache_path = None
+    else:
+      cache_prefix = kwargs.pop('cache_prefix', None)
+      cache_path = cache_path_for_file_path(fn, cache_prefix)
+
+    if cache_path and os.path.exists(cache_path):
+      with open(cache_path, "rb") as cache_file:
+        cache_value = pickle.load(cache_file)
+    else:
+      cache_value = func(fn, *args, **kwargs)
+
+      if cache_path:
+        with atomic_write_in_dir(cache_path, mode="wb", overwrite=True) as cache_file:
+          pickle.dump(cache_value, cache_file, -1)
+
+    return cache_value
+
+  return cache_inner
+
+
+@cache_fn
+def index_stream(fn, typ):
+  assert typ in ("hevc", )
+
+  with FileReader(fn) as f:
+    assert os.path.exists(f.name), fn
+    index, prefix = vidindex(f.name, typ)
+    probe = ffprobe(f.name, typ)
+
+  return {
+    'index': index,
+    'global_prefix': prefix,
+    'probe': probe
+  }
+
+
+def index_videos(camera_paths, cache_prefix=None):
+  """Requires that paths in camera_paths are contiguous and of the same type."""
+  if len(camera_paths) < 1:
+    raise ValueError("must provide at least one video to index")
+
+  frame_type = fingerprint_video(camera_paths[0])
+  for fn in camera_paths:
+    index_video(fn, frame_type, cache_prefix)
+
+
+def index_video(fn, frame_type=None, cache_prefix=None):
+  cache_path = cache_path_for_file_path(fn, cache_prefix)
+
+  if os.path.exists(cache_path):
+    return
+
+  if frame_type is None:
+    frame_type = fingerprint_video(fn[0])
+
+  if frame_type == FrameType.h265_stream:
+    index_stream(fn, "hevc", cache_prefix=cache_prefix)
+  else:
+    raise NotImplementedError("Only h265 supported")
+
+
+def get_video_index(fn, frame_type, cache_prefix=None):
+  cache_path = cache_path_for_file_path(fn, cache_prefix)
+
+  if not os.path.exists(cache_path):
+    index_video(fn, frame_type, cache_prefix)
+
+  if not os.path.exists(cache_path):
+    return None
+  with open(cache_path, "rb") as cache_file:
+    return pickle.load(cache_file)
+
+
+def read_file_check_size(f, sz, cookie):
+  buff = bytearray(sz)
+  bytes_read = f.readinto(buff)
+  assert bytes_read == sz, (bytes_read, sz)
+  return buff
+
+
+def rgb24toyuv420(rgb):
+  yuv_from_rgb = np.array([[ 0.299     ,  0.587     ,  0.114      ],
+                           [-0.14714119, -0.28886916,  0.43601035 ],
+                           [ 0.61497538, -0.51496512, -0.10001026 ]])
+  img = np.dot(rgb.reshape(-1, 3), yuv_from_rgb.T).reshape(rgb.shape)
+
+  y_len = img.shape[0] * img.shape[1]
+  uv_len = y_len // 4
+
+  ys = img[:, :, 0]
+  us = (img[::2, ::2, 1] + img[1::2, ::2, 1] + img[::2, 1::2, 1] + img[1::2, 1::2, 1]) / 4 + 128
+  vs = (img[::2, ::2, 2] + img[1::2, ::2, 2] + img[::2, 1::2, 2] + img[1::2, 1::2, 2]) / 4 + 128
+
+  yuv420 = np.empty(y_len + 2 * uv_len, dtype=img.dtype)
+  yuv420[:y_len] = ys.reshape(-1)
+  yuv420[y_len:y_len + uv_len] = us.reshape(-1)
+  yuv420[y_len + uv_len:y_len + 2 * uv_len] = vs.reshape(-1)
+
+  return yuv420.clip(0, 255).astype('uint8')
+
+
+def decompress_video_data(rawdat, vid_fmt, w, h, pix_fmt):
+  # using a tempfile is much faster than proc.communicate for some reason
+
+  with tempfile.TemporaryFile() as tmpf:
+    tmpf.write(rawdat)
+    tmpf.seek(0)
+
+    threads = os.getenv("FFMPEG_THREADS", "0")
+    cuda = os.getenv("FFMPEG_CUDA", "0") == "1"
+    proc = subprocess.Popen(
+      ["ffmpeg",
+       "-threads", threads,
+       "-hwaccel", "none" if not cuda else "cuda",
+       "-c:v", "hevc",
+       "-vsync", "0",
+       "-f", vid_fmt,
+       "-flags2", "showall",
+       "-i", "pipe:0",
+       "-threads", threads,
+       "-f", "rawvideo",
+       "-pix_fmt", pix_fmt,
+       "pipe:1"],
+      stdin=tmpf, stdout=subprocess.PIPE, stderr=open("/dev/null"))
+
+    # dat = proc.communicate()[0]
+    dat = proc.stdout.read()
+    if proc.wait() != 0:
+      raise DataUnreadableError("ffmpeg failed")
+
+  if pix_fmt == "rgb24":
+    ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, h, w, 3)
+  elif pix_fmt == "yuv420p":
+    ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, (h*w*3//2))
+  elif pix_fmt == "yuv444p":
+    ret = np.frombuffer(dat, dtype=np.uint8).reshape(-1, 3, h, w)
+  else:
+    raise NotImplementedError
+
+  return ret
+
+
+class BaseFrameReader:
+  # properties: frame_type, frame_count, w, h
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, *args):
+    self.close()
+
+  def close(self):
+    pass
+
+  def get(self, num, count=1, pix_fmt="yuv420p"):
+    raise NotImplementedError
+
+
+def FrameReader(fn, cache_prefix=None, readahead=False, readbehind=False, index_data=None):
+  frame_type = fingerprint_video(fn)
+  if frame_type == FrameType.raw:
+    return RawFrameReader(fn)
+  elif frame_type in (FrameType.h265_stream,):
+    if not index_data:
+      index_data = get_video_index(fn, frame_type, cache_prefix)
+    return StreamFrameReader(fn, frame_type, index_data, readahead=readahead, readbehind=readbehind)
+  else:
+    raise NotImplementedError(frame_type)
+
+
+class RawData:
+  def __init__(self, f):
+    self.f = _io.FileIO(f, 'rb')
+    self.lenn = struct.unpack("I", self.f.read(4))[0]
+    self.count = os.path.getsize(f) / (self.lenn+4)
+
+  def read(self, i):
+    self.f.seek((self.lenn+4)*i + 4)
+    return self.f.read(self.lenn)
+
+
+class RawFrameReader(BaseFrameReader):
+  def __init__(self, fn):
+    # raw camera
+    self.fn = fn
+    self.frame_type = FrameType.raw
+    self.rawfile = RawData(self.fn)
+    self.frame_count = self.rawfile.count
+    self.w, self.h = 640, 480
+
+  def load_and_debayer(self, img):
+    img = np.frombuffer(img, dtype='uint8').reshape(960, 1280)
+    cimg = np.dstack([img[0::2, 1::2], ((img[0::2, 0::2].astype("uint16") + img[1::2, 1::2].astype("uint16")) >> 1).astype("uint8"), img[1::2, 0::2]])
+    return cimg
+
+  def get(self, num, count=1, pix_fmt="yuv420p"):
+    assert self.frame_count is not None
+    assert num+count <= self.frame_count
+
+    if pix_fmt not in ("yuv420p", "rgb24"):
+      raise ValueError("Unsupported pixel format %r" % pix_fmt)
+
+    app = []
+    for i in range(num, num+count):
+      dat = self.rawfile.read(i)
+      rgb_dat = self.load_and_debayer(dat)
+      if pix_fmt == "rgb24":
+        app.append(rgb_dat)
+      elif pix_fmt == "yuv420p":
+        app.append(rgb24toyuv420(rgb_dat))
+      else:
+        raise NotImplementedError
+
+    return app
+
+
+class VideoStreamDecompressor:
+  def __init__(self, fn, vid_fmt, w, h, pix_fmt):
+    self.fn = fn
+    self.vid_fmt = vid_fmt
+    self.w = w
+    self.h = h
+    self.pix_fmt = pix_fmt
+
+    if pix_fmt == "yuv420p":
+      self.out_size = w*h*3//2  # yuv420p
+    elif pix_fmt in ("rgb24", "yuv444p"):
+      self.out_size = w*h*3
+    else:
+      raise NotImplementedError
+
+    self.proc = None
+    self.t = threading.Thread(target=self.write_thread)
+    self.t.daemon = True
+
+  def write_thread(self):
+    try:
+      with FileReader(self.fn) as f:
+        while True:
+          r = f.read(1024*1024)
+          if len(r) == 0:
+            break
+          self.proc.stdin.write(r)
+    finally:
+      self.proc.stdin.close()
+
+  def read(self):
+    threads = os.getenv("FFMPEG_THREADS", "0")
+    cuda = os.getenv("FFMPEG_CUDA", "0") == "1"
+    cmd = [
+      "ffmpeg",
+      "-threads", threads,
+      "-hwaccel", "none" if not cuda else "cuda",
+      "-c:v", "hevc",
+      # "-avioflags", "direct",
+      "-analyzeduration", "0",
+      "-probesize", "32",
+      "-flush_packets", "0",
+      # "-fflags", "nobuffer",
+      "-vsync", "0",
+      "-f", self.vid_fmt,
+      "-i", "pipe:0",
+      "-threads", threads,
+      "-f", "rawvideo",
+      "-pix_fmt", self.pix_fmt,
+      "pipe:1"
+    ]
+    self.proc = subprocess.Popen(cmd, stdin=subprocess.PIPE, stdout=subprocess.PIPE, stderr=subprocess.DEVNULL)
+    try:
+      self.t.start()
+
+      while True:
+        dat = self.proc.stdout.read(self.out_size)
+        if len(dat) == 0:
+          break
+        assert len(dat) == self.out_size
+        if self.pix_fmt == "rgb24":
+          ret = np.frombuffer(dat, dtype=np.uint8).reshape((self.h, self.w, 3))
+        elif self.pix_fmt == "yuv420p":
+          ret = np.frombuffer(dat, dtype=np.uint8)
+        elif self.pix_fmt == "yuv444p":
+          ret = np.frombuffer(dat, dtype=np.uint8).reshape((3, self.h, self.w))
+        else:
+          assert False
+        yield ret
+
+      result_code = self.proc.wait()
+      assert result_code == 0, result_code
+    finally:
+      self.proc.kill()
+      self.t.join()
+
+class StreamGOPReader(GOPReader):
+  def __init__(self, fn, frame_type, index_data):
+    assert frame_type == FrameType.h265_stream
+
+    self.fn = fn
+
+    self.frame_type = frame_type
+    self.frame_count = None
+    self.w, self.h = None, None
+
+    self.prefix = None
+    self.index = None
+
+    self.index = index_data['index']
+    self.prefix = index_data['global_prefix']
+    probe = index_data['probe']
+
+    self.prefix_frame_data = None
+    self.num_prefix_frames = 0
+    self.vid_fmt = "hevc"
+
+    i = 0
+    while i < self.index.shape[0] and self.index[i, 0] != HEVC_SLICE_I:
+      i += 1
+    self.first_iframe = i
+
+    assert self.first_iframe == 0
+
+    self.frame_count = len(self.index) - 1
+
+    self.w = probe['streams'][0]['width']
+    self.h = probe['streams'][0]['height']
+
+  def _lookup_gop(self, num):
+    frame_b = num
+    while frame_b > 0 and self.index[frame_b, 0] != HEVC_SLICE_I:
+      frame_b -= 1
+
+    frame_e = num + 1
+    while frame_e < (len(self.index) - 1) and self.index[frame_e, 0] != HEVC_SLICE_I:
+      frame_e += 1
+
+    offset_b = self.index[frame_b, 1]
+    offset_e = self.index[frame_e, 1]
+
+    return (frame_b, frame_e, offset_b, offset_e)
+
+  def get_gop(self, num):
+    frame_b, frame_e, offset_b, offset_e = self._lookup_gop(num)
+    assert frame_b <= num < frame_e
+
+    num_frames = frame_e - frame_b
+
+    with FileReader(self.fn) as f:
+      f.seek(offset_b)
+      rawdat = f.read(offset_e - offset_b)
+
+      if num < self.first_iframe:
+        assert self.prefix_frame_data
+        rawdat = self.prefix_frame_data + rawdat
+
+      rawdat = self.prefix + rawdat
+
+    skip_frames = 0
+    if num < self.first_iframe:
+      skip_frames = self.num_prefix_frames
+
+    return frame_b, num_frames, skip_frames, rawdat
+
+
+class GOPFrameReader(BaseFrameReader):
+  #FrameReader with caching and readahead for formats that are group-of-picture based
+
+  def __init__(self, readahead=False, readbehind=False):
+    self.open_ = True
+
+    self.readahead = readahead
+    self.readbehind = readbehind
+    self.frame_cache = LRU(64)
+
+    if self.readahead:
+      self.cache_lock = threading.RLock()
+      self.readahead_last = None
+      self.readahead_len = 30
+      self.readahead_c = threading.Condition()
+      self.readahead_thread = threading.Thread(target=self._readahead_thread)
+      self.readahead_thread.daemon = True
+      self.readahead_thread.start()
+    else:
+      self.cache_lock = DoNothingContextManager()
+
+  def close(self):
+    if not self.open_:
+      return
+    self.open_ = False
+
+    if self.readahead:
+      self.readahead_c.acquire()
+      self.readahead_c.notify()
+      self.readahead_c.release()
+      self.readahead_thread.join()
+
+  def _readahead_thread(self):
+    while True:
+      self.readahead_c.acquire()
+      try:
+        if not self.open_:
+          break
+        self.readahead_c.wait()
+      finally:
+        self.readahead_c.release()
+      if not self.open_:
+        break
+      assert self.readahead_last
+      num, pix_fmt = self.readahead_last
+
+      if self.readbehind:
+        for k in range(num - 1, max(0, num - self.readahead_len), -1):
+          self._get_one(k, pix_fmt)
+      else:
+        for k in range(num, min(self.frame_count, num + self.readahead_len)):
+          self._get_one(k, pix_fmt)
+
+  def _get_one(self, num, pix_fmt):
+    assert num < self.frame_count
+
+    if (num, pix_fmt) in self.frame_cache:
+      return self.frame_cache[(num, pix_fmt)]
+
+    with self.cache_lock:
+      if (num, pix_fmt) in self.frame_cache:
+        return self.frame_cache[(num, pix_fmt)]
+
+      frame_b, num_frames, skip_frames, rawdat = self.get_gop(num)
+
+      ret = decompress_video_data(rawdat, self.vid_fmt, self.w, self.h, pix_fmt)
+      ret = ret[skip_frames:]
+      assert ret.shape[0] == num_frames
+
+      for i in range(ret.shape[0]):
+        self.frame_cache[(frame_b+i, pix_fmt)] = ret[i]
+
+      return self.frame_cache[(num, pix_fmt)]
+
+  def get(self, num, count=1, pix_fmt="yuv420p"):
+    assert self.frame_count is not None
+
+    if num + count > self.frame_count:
+      raise ValueError("{} > {}".format(num + count, self.frame_count))
+
+    if pix_fmt not in ("yuv420p", "rgb24", "yuv444p"):
+      raise ValueError("Unsupported pixel format %r" % pix_fmt)
+
+    ret = [self._get_one(num + i, pix_fmt) for i in range(count)]
+
+    if self.readahead:
+      self.readahead_last = (num+count, pix_fmt)
+      self.readahead_c.acquire()
+      self.readahead_c.notify()
+      self.readahead_c.release()
+
+    return ret
+
+
+class StreamFrameReader(StreamGOPReader, GOPFrameReader):
+  def __init__(self, fn, frame_type, index_data, readahead=False, readbehind=False):
+    StreamGOPReader.__init__(self, fn, frame_type, index_data)
+    GOPFrameReader.__init__(self, readahead, readbehind)
+
+
+def GOPFrameIterator(gop_reader, pix_fmt):
+  dec = VideoStreamDecompressor(gop_reader.fn, gop_reader.vid_fmt, gop_reader.w, gop_reader.h, pix_fmt)
+  for frame in dec.read():
+    yield frame
+
+
+def FrameIterator(fn, pix_fmt, **kwargs):
+  fr = FrameReader(fn, **kwargs)
+  if isinstance(fr, GOPReader):
+    for v in GOPFrameIterator(fr, pix_fmt):
+      yield v
+  else:
+    for i in range(fr.frame_count):
+      yield fr.get(i, pix_fmt=pix_fmt)[0]
diff --git a/tools/lib/kbhit.py b/tools/lib/kbhit.py
new file mode 100644
index 000000000..ac048fda9
--- /dev/null
+++ b/tools/lib/kbhit.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+import sys
+import termios
+import atexit
+from select import select
+
+STDIN_FD = sys.stdin.fileno()
+
+class KBHit:
+  def __init__(self) -> None:
+    ''' Creates a KBHit object that you can call to do various keyboard things.
+    '''
+
+    self.set_kbhit_terminal()
+
+  def set_kbhit_terminal(self) -> None:
+    ''' Save old terminal settings for closure, remove ICANON & ECHO flags.
+    '''
+
+    # Save the terminal settings
+    self.old_term = termios.tcgetattr(STDIN_FD)
+    self.new_term = self.old_term.copy()
+
+    # New terminal setting unbuffered
+    self.new_term[3] &= ~(termios.ICANON | termios.ECHO)  # type: ignore
+    termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, self.new_term)
+
+    # Support normal-terminal reset at exit
+    atexit.register(self.set_normal_term)
+
+  def set_normal_term(self) -> None:
+    ''' Resets to normal terminal. On Windows this is a no-op.
+    '''
+
+    termios.tcsetattr(STDIN_FD, termios.TCSAFLUSH, self.old_term)
+
+  @staticmethod
+  def getch() -> str:
+    ''' Returns a keyboard character after kbhit() has been called.
+      Should not be called in the same program as getarrow().
+    '''
+    return sys.stdin.read(1)
+
+  @staticmethod
+  def getarrow() -> int:
+    ''' Returns an arrow-key code after kbhit() has been called. Codes are
+    0 : up
+    1 : right
+    2 : down
+    3 : left
+    Should not be called in the same program as getch().
+    '''
+
+    c = sys.stdin.read(3)[2]
+    vals = [65, 67, 66, 68]
+
+    return vals.index(ord(c))
+
+  @staticmethod
+  def kbhit():
+    ''' Returns True if keyboard character was hit, False otherwise.
+    '''
+    return select([sys.stdin], [], [], 0)[0] != []
+
+
+# Test
+if __name__ == "__main__":
+
+  kb = KBHit()
+
+  print('Hit any key, or ESC to exit')
+
+  while True:
+
+    if kb.kbhit():
+      c = kb.getch()
+      if c == '\x1b':  # ESC
+        break
+      print(c)
+
+  kb.set_normal_term()
diff --git a/tools/lib/lazy_property.py b/tools/lib/lazy_property.py
new file mode 100644
index 000000000..85c038f28
--- /dev/null
+++ b/tools/lib/lazy_property.py
@@ -0,0 +1,12 @@
+class lazy_property(object):
+  """Defines a property whose value will be computed only once and as needed.
+
+     This can only be used on instance methods.
+  """
+  def __init__(self, func):
+    self._func = func
+
+  def __get__(self, obj_self, cls):
+    value = self._func(obj_self)
+    setattr(obj_self, self._func.__name__, value)
+    return value
diff --git a/tools/lib/logreader.py b/tools/lib/logreader.py
new file mode 100755
index 000000000..01c666fd1
--- /dev/null
+++ b/tools/lib/logreader.py
@@ -0,0 +1,118 @@
+#!/usr/bin/env python3
+import os
+import sys
+import bz2
+import urllib.parse
+import capnp
+
+try:
+  from xx.chffr.lib.filereader import FileReader
+except ImportError:
+  from tools.lib.filereader import FileReader
+from cereal import log as capnp_log
+
+# this is an iterator itself, and uses private variables from LogReader
+class MultiLogIterator(object):
+  def __init__(self, log_paths, wraparound=True):
+    self._log_paths = log_paths
+    self._wraparound = wraparound
+
+    self._first_log_idx = next(i for i in range(len(log_paths)) if log_paths[i] is not None)
+    self._current_log = self._first_log_idx
+    self._idx = 0
+    self._log_readers = [None]*len(log_paths)
+    self.start_time = self._log_reader(self._first_log_idx)._ts[0]
+
+  def _log_reader(self, i):
+    if self._log_readers[i] is None and self._log_paths[i] is not None:
+      log_path = self._log_paths[i]
+      print("LogReader:%s" % log_path)
+      self._log_readers[i] = LogReader(log_path)
+
+    return self._log_readers[i]
+
+  def __iter__(self):
+    return self
+
+  def _inc(self):
+    lr = self._log_reader(self._current_log)
+    if self._idx < len(lr._ents)-1:
+      self._idx += 1
+    else:
+      self._idx = 0
+      self._current_log = next(i for i in range(self._current_log + 1, len(self._log_readers) + 1)
+                               if i == len(self._log_readers) or self._log_paths[i] is not None)
+      # wraparound
+      if self._current_log == len(self._log_readers):
+        if self._wraparound:
+          self._current_log = self._first_log_idx
+        else:
+          raise StopIteration
+
+  def __next__(self):
+    while 1:
+      lr = self._log_reader(self._current_log)
+      ret = lr._ents[self._idx]
+      self._inc()
+      return ret
+
+  def tell(self):
+    # returns seconds from start of log
+    return (self._log_reader(self._current_log)._ts[self._idx] - self.start_time) * 1e-9
+
+  def seek(self, ts):
+    # seek to nearest minute
+    minute = int(ts/60)
+    if minute >= len(self._log_paths) or self._log_paths[minute] is None:
+      return False
+
+    self._current_log = minute
+
+    # HACK: O(n) seek afterward
+    self._idx = 0
+    while self.tell() < ts:
+      self._inc()
+    return True
+
+
+class LogReader(object):
+  def __init__(self, fn, canonicalize=True, only_union_types=False):
+    data_version = None
+    _, ext = os.path.splitext(urllib.parse.urlparse(fn).path)
+    with FileReader(fn) as f:
+      dat = f.read()
+
+    if ext == "":
+      # old rlogs weren't bz2 compressed
+      ents = capnp_log.Event.read_multiple_bytes(dat)
+    elif ext == ".bz2":
+      dat = bz2.decompress(dat)
+      ents = capnp_log.Event.read_multiple_bytes(dat)
+    else:
+      raise Exception(f"unknown extension {ext}")
+
+    self._ents = list(ents)
+    self._ts = [x.logMonoTime for x in self._ents]
+    self.data_version = data_version
+    self._only_union_types = only_union_types
+
+  def __iter__(self):
+    for ent in self._ents:
+      if self._only_union_types:
+        try:
+          ent.which()
+          yield ent
+        except capnp.lib.capnp.KjException:
+          pass
+      else:
+        yield ent
+
+if __name__ == "__main__":
+  import codecs
+  # capnproto <= 0.8.0 throws errors converting byte data to string
+  # below line catches those errors and replaces the bytes with \x__
+  codecs.register_error("strict", codecs.backslashreplace_errors)
+  log_path = sys.argv[1]
+  lr = LogReader(log_path)
+  for msg in lr:
+    print(msg)
diff --git a/tools/lib/route.py b/tools/lib/route.py
new file mode 100644
index 000000000..6bb22933e
--- /dev/null
+++ b/tools/lib/route.py
@@ -0,0 +1,185 @@
+import os
+import re
+from urllib.parse import urlparse
+from collections import defaultdict
+from itertools import chain
+
+from tools.lib.auth_config import get_token
+from tools.lib.api import CommaApi
+
+SEGMENT_NAME_RE = r'[a-z0-9]{16}[|_][0-9]{4}-[0-9]{2}-[0-9]{2}--[0-9]{2}-[0-9]{2}-[0-9]{2}--[0-9]+'
+EXPLORER_FILE_RE = r'^({})--([a-z]+\.[a-z0-9]+)$'.format(SEGMENT_NAME_RE)
+OP_SEGMENT_DIR_RE = r'^({})$'.format(SEGMENT_NAME_RE)
+
+QLOG_FILENAMES = ['qlog.bz2']
+QCAMERA_FILENAMES = ['qcamera.ts']
+LOG_FILENAMES = ['rlog.bz2', 'raw_log.bz2']
+CAMERA_FILENAMES = ['fcamera.hevc', 'video.hevc']
+DCAMERA_FILENAMES = ['dcamera.hevc']
+ECAMERA_FILENAMES = ['ecamera.hevc']
+
+class Route(object):
+  def __init__(self, route_name, data_dir=None):
+    self.route_name = route_name.replace('_', '|')
+    if data_dir is not None:
+      self._segments = self._get_segments_local(data_dir)
+    else:
+      self._segments = self._get_segments_remote()
+    self.max_seg_number = self._segments[-1].canonical_name.segment_num
+
+  @property
+  def segments(self):
+    return self._segments
+
+  def log_paths(self):
+    log_path_by_seg_num = {s.canonical_name.segment_num: s.log_path for s in self._segments}
+    return [log_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  def qlog_paths(self):
+    qlog_path_by_seg_num = {s.canonical_name.segment_num: s.qlog_path for s in self._segments}
+    return [qlog_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  def camera_paths(self):
+    camera_path_by_seg_num = {s.canonical_name.segment_num: s.camera_path for s in self._segments}
+    return [camera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  def dcamera_paths(self):
+    dcamera_path_by_seg_num = {s.canonical_name.segment_num: s.dcamera_path for s in self._segments}
+    return [dcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  def ecamera_paths(self):
+    ecamera_path_by_seg_num = {s.canonical_name.segment_num: s.ecamera_path for s in self._segments}
+    return [ecamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  def qcamera_paths(self):
+    qcamera_path_by_seg_num = {s.canonical_name.segment_num: s.qcamera_path for s in self._segments}
+    return [qcamera_path_by_seg_num.get(i, None) for i in range(self.max_seg_number+1)]
+
+  # TODO: refactor this, it's super repetitive
+  def _get_segments_remote(self):
+    api = CommaApi(get_token())
+    route_files = api.get('v1/route/' + self.route_name + '/files')
+
+    segments = {}
+    for url in chain.from_iterable(route_files.values()):
+      _, dongle_id, time_str, segment_num, fn = urlparse(url).path.rsplit('/', maxsplit=4)
+      segment_name = f'{dongle_id}|{time_str}--{segment_num}'
+      if segments.get(segment_name):
+        segments[segment_name] = RouteSegment(
+          segment_name,
+          url if fn in LOG_FILENAMES else segments[segment_name].log_path,
+          url if fn in QLOG_FILENAMES else segments[segment_name].qlog_path,
+          url if fn in CAMERA_FILENAMES else segments[segment_name].camera_path,
+          url if fn in DCAMERA_FILENAMES else segments[segment_name].dcamera_path,
+          url if fn in ECAMERA_FILENAMES else segments[segment_name].ecamera_path,
+          url if fn in QCAMERA_FILENAMES else segments[segment_name].qcamera_path,
+        )
+      else:
+        segments[segment_name] = RouteSegment(
+          segment_name,
+          url if fn in LOG_FILENAMES else None,
+          url if fn in QLOG_FILENAMES else None,
+          url if fn in CAMERA_FILENAMES else None,
+          url if fn in DCAMERA_FILENAMES else None,
+          url if fn in ECAMERA_FILENAMES else None,
+          url if fn in QCAMERA_FILENAMES else None,
+        )
+
+    return sorted(segments.values(), key=lambda seg: seg.canonical_name.segment_num)
+
+  def _get_segments_local(self, data_dir):
+    files = os.listdir(data_dir)
+    segment_files = defaultdict(list)
+
+    for f in files:
+      fullpath = os.path.join(data_dir, f)
+      explorer_match = re.match(EXPLORER_FILE_RE, f)
+      op_match = re.match(OP_SEGMENT_DIR_RE, f)
+
+      if explorer_match:
+        segment_name, fn = explorer_match.groups()
+        if segment_name.replace('_', '|').startswith(self.route_name):
+          segment_files[segment_name].append((fullpath, fn))
+      elif op_match and os.path.isdir(fullpath):
+        segment_name, = op_match.groups()
+        if segment_name.startswith(self.route_name):
+          for seg_f in os.listdir(fullpath):
+            segment_files[segment_name].append((os.path.join(fullpath, seg_f), seg_f))
+      elif f == self.route_name:
+        for seg_num in os.listdir(fullpath):
+          if not seg_num.isdigit():
+            continue
+
+          segment_name = '{}--{}'.format(self.route_name, seg_num)
+          for seg_f in os.listdir(os.path.join(fullpath, seg_num)):
+            segment_files[segment_name].append((os.path.join(fullpath, seg_num, seg_f), seg_f))
+
+    segments = []
+    for segment, files in segment_files.items():
+
+      try:
+        log_path = next(path for path, filename in files if filename in LOG_FILENAMES)
+      except StopIteration:
+        log_path = None
+
+      try:
+        qlog_path = next(path for path, filename in files if filename in QLOG_FILENAMES)
+      except StopIteration:
+        qlog_path = None
+
+      try:
+        camera_path = next(path for path, filename in files if filename in CAMERA_FILENAMES)
+      except StopIteration:
+        camera_path = None
+
+      try:
+        dcamera_path = next(path for path, filename in files if filename in DCAMERA_FILENAMES)
+      except StopIteration:
+        dcamera_path = None
+
+      try:
+        ecamera_path = next(path for path, filename in files if filename in ECAMERA_FILENAMES)
+      except StopIteration:
+        ecamera_path = None
+
+      try:
+        qcamera_path = next(path for path, filename in files if filename in QCAMERA_FILENAMES)
+      except StopIteration:
+        qcamera_path = None
+
+      segments.append(RouteSegment(segment, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path))
+
+    if len(segments) == 0:
+      raise ValueError('Could not find segments for route {} in data directory {}'.format(self.route_name, data_dir))
+    return sorted(segments, key=lambda seg: seg.canonical_name.segment_num)
+
+class RouteSegment(object):
+  def __init__(self, name, log_path, qlog_path, camera_path, dcamera_path, ecamera_path, qcamera_path):
+    self._name = RouteSegmentName(name)
+    self.log_path = log_path
+    self.qlog_path = qlog_path
+    self.camera_path = camera_path
+    self.dcamera_path = dcamera_path
+    self.ecamera_path = ecamera_path
+    self.qcamera_path = qcamera_path
+
+  @property
+  def name(self):
+    return str(self._name)
+
+  @property
+  def canonical_name(self):
+    return self._name
+
+class RouteSegmentName(object):
+  def __init__(self, name_str):
+    self._segment_name_str = name_str
+    self._route_name_str, num_str = self._segment_name_str.rsplit("--", 1)
+    self._num = int(num_str)
+
+  @property
+  def segment_num(self):
+    return self._num
+
+  def __str__(self):
+    return self._segment_name_str
diff --git a/tools/lib/tests/__init__.py b/tools/lib/tests/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/tools/lib/tests/test_caching.py b/tools/lib/tests/test_caching.py
new file mode 100644
index 000000000..953b47e95
--- /dev/null
+++ b/tools/lib/tests/test_caching.py
@@ -0,0 +1,69 @@
+#!/usr/bin/env python3
+import os
+import shutil
+import unittest
+
+os.environ["COMMA_CACHE"] = "/tmp/__test_cache__"
+from tools.lib.url_file import URLFile, CACHE_DIR
+
+
+class TestFileDownload(unittest.TestCase):
+
+  def compare_loads(self, url, start=0, length=None):
+    """Compares range between cached and non cached version"""
+    shutil.rmtree(CACHE_DIR)
+
+    file_cached = URLFile(url, cache=True)
+    file_downloaded = URLFile(url, cache=False)
+
+    file_cached.seek(start)
+    file_downloaded.seek(start)
+
+    self.assertEqual(file_cached.get_length(), file_downloaded.get_length())
+    self.assertLessEqual(length + start if length is not None else 0, file_downloaded.get_length())
+
+    response_cached = file_cached.read(ll=length)
+    response_downloaded = file_downloaded.read(ll=length)
+
+    self.assertEqual(response_cached, response_downloaded)
+
+    # Now test with cache in place
+    file_cached = URLFile(url, cache=True)
+    file_cached.seek(start)
+    response_cached = file_cached.read(ll=length)
+
+    self.assertEqual(file_cached.get_length(), file_downloaded.get_length())
+    self.assertEqual(response_cached, response_downloaded)
+
+  def test_small_file(self):
+    # Make sure we don't force cache
+    os.environ["FILEREADER_CACHE"] = "0"
+    small_file_url = "https://raw.githubusercontent.com/commaai/openpilot/master/docs/SAFETY.md"
+    #  If you want large file to be larger than a chunk
+    #  large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/fcamera.hevc"
+
+    #  Load full small file
+    self.compare_loads(small_file_url)
+
+    file_small = URLFile(small_file_url)
+    length = file_small.get_length()
+
+    self.compare_loads(small_file_url, length - 100, 100)
+    self.compare_loads(small_file_url, 50, 100)
+
+    #  Load small file 100 bytes at a time
+    for i in range(length // 100):
+      self.compare_loads(small_file_url, 100 * i, 100)
+
+  def test_large_file(self):
+    large_file_url = "https://commadataci.blob.core.windows.net/openpilotci/0375fdf7b1ce594d/2019-06-13--08-32-25/3/qlog.bz2"
+    #  Load the end 100 bytes of both files
+    file_large = URLFile(large_file_url)
+    length = file_large.get_length()
+
+    self.compare_loads(large_file_url, length - 100, 100)
+    self.compare_loads(large_file_url)
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/tools/lib/tests/test_readers.py b/tools/lib/tests/test_readers.py
new file mode 100755
index 000000000..1d8918ba5
--- /dev/null
+++ b/tools/lib/tests/test_readers.py
@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+import unittest
+import requests
+import tempfile
+
+from collections import defaultdict
+import numpy as np
+from tools.lib.framereader import FrameReader
+from tools.lib.logreader import LogReader
+
+
+class TestReaders(unittest.TestCase):
+  @unittest.skip("skip for bandwith reasons")
+  def test_logreader(self):
+    def _check_data(lr):
+      hist = defaultdict(int)
+      for l in lr:
+        hist[l.which()] += 1
+
+      self.assertEqual(hist['carControl'], 6000)
+      self.assertEqual(hist['logMessage'], 6857)
+
+    with tempfile.NamedTemporaryFile(suffix=".bz2") as fp:
+      r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true")
+      fp.write(r.content)
+      fp.flush()
+
+      lr_file = LogReader(fp.name)
+      _check_data(lr_file)
+
+    lr_url = LogReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/raw_log.bz2?raw=true")
+    _check_data(lr_url)
+
+  @unittest.skip("skip for bandwith reasons")
+  def test_framereader(self):
+    def _check_data(f):
+      self.assertEqual(f.frame_count, 1200)
+      self.assertEqual(f.w, 1164)
+      self.assertEqual(f.h, 874)
+
+      frame_first_30 = f.get(0, 30)
+      self.assertEqual(len(frame_first_30), 30)
+
+      print(frame_first_30[15])
+
+      print("frame_0")
+      frame_0 = f.get(0, 1)
+      frame_15 = f.get(15, 1)
+
+      print(frame_15[0])
+
+      assert np.all(frame_first_30[0] == frame_0[0])
+      assert np.all(frame_first_30[15] == frame_15[0])
+
+    with tempfile.NamedTemporaryFile(suffix=".hevc") as fp:
+      r = requests.get("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true")
+      fp.write(r.content)
+      fp.flush()
+
+      fr_file = FrameReader(fp.name)
+      _check_data(fr_file)
+
+    fr_url = FrameReader("https://github.com/commaai/comma2k19/blob/master/Example_1/b0c9d2329ad1606b%7C2018-08-02--08-34-47/40/video.hevc?raw=true")
+    _check_data(fr_url)
+
+if __name__ == "__main__":
+  unittest.main()
diff --git a/tools/lib/url_file.py b/tools/lib/url_file.py
new file mode 100644
index 000000000..2161770d0
--- /dev/null
+++ b/tools/lib/url_file.py
@@ -0,0 +1,198 @@
+# pylint: skip-file
+
+import os
+import time
+import tempfile
+import threading
+import urllib.parse
+import pycurl
+from hashlib import sha256
+from io import BytesIO
+from tenacity import retry, wait_random_exponential, stop_after_attempt
+from common.file_helpers import mkdirs_exists_ok, atomic_write_in_dir
+#  Cache chunk size
+K = 1000
+CHUNK_SIZE = 1000 * K
+
+CACHE_DIR = os.environ.get("COMMA_CACHE", "/tmp/comma_download_cache/")
+
+
+def hash_256(link):
+  hsh = str(sha256((link.split("?")[0]).encode('utf-8')).hexdigest())
+  return hsh
+
+
+class URLFile(object):
+  _tlocal = threading.local()
+
+  def __init__(self, url, debug=False, cache=None):
+    self._url = url
+    self._pos = 0
+    self._length = None
+    self._local_file = None
+    self._debug = debug
+    #  True by default, false if FILEREADER_CACHE is defined, but can be overwritten by the cache input
+    self._force_download = not int(os.environ.get("FILEREADER_CACHE", "0"))
+    if cache is not None:
+      self._force_download = not cache
+
+    try:
+      self._curl = self._tlocal.curl
+    except AttributeError:
+      self._curl = self._tlocal.curl = pycurl.Curl()
+    mkdirs_exists_ok(CACHE_DIR)
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, exc_type, exc_value, traceback):
+    if self._local_file is not None:
+      os.remove(self._local_file.name)
+      self._local_file.close()
+      self._local_file = None
+
+  @retry(wait=wait_random_exponential(multiplier=1, max=5), stop=stop_after_attempt(3), reraise=True)
+  def get_length_online(self):
+    c = self._curl
+    c.reset()
+    c.setopt(pycurl.NOSIGNAL, 1)
+    c.setopt(pycurl.TIMEOUT_MS, 500000)
+    c.setopt(pycurl.FOLLOWLOCATION, True)
+    c.setopt(pycurl.URL, self._url)
+    c.setopt(c.NOBODY, 1)
+    c.perform()
+    length = int(c.getinfo(c.CONTENT_LENGTH_DOWNLOAD))
+    c.reset()
+    return length
+
+  def get_length(self):
+    if self._length is not None:
+      return self._length
+    file_length_path = os.path.join(CACHE_DIR, hash_256(self._url) + "_length")
+    if os.path.exists(file_length_path) and not self._force_download:
+      with open(file_length_path, "r") as file_length:
+          content = file_length.read()
+          self._length = int(content)
+          return self._length
+
+    self._length = self.get_length_online()
+    if not self._force_download:
+      with atomic_write_in_dir(file_length_path, mode="w") as file_length:
+        file_length.write(str(self._length))
+    return self._length
+
+  def read(self, ll=None):
+    if self._force_download:
+      return self.read_aux(ll=ll)
+
+    file_begin = self._pos
+    file_end = self._pos + ll if ll is not None else self.get_length()
+    #  We have to align with chunks we store. Position is the begginiing of the latest chunk that starts before or at our file
+    position = (file_begin // CHUNK_SIZE) * CHUNK_SIZE
+    response = b""
+    while True:
+      self._pos = position
+      chunk_number = self._pos / CHUNK_SIZE
+      file_name = hash_256(self._url) + "_" + str(chunk_number)
+      full_path = os.path.join(CACHE_DIR, str(file_name))
+      data = None
+      #  If we don't have a file, download it
+      if not os.path.exists(full_path):
+        data = self.read_aux(ll=CHUNK_SIZE)
+        with atomic_write_in_dir(full_path, mode="wb") as new_cached_file:
+          new_cached_file.write(data)
+      else:
+        with open(full_path, "rb") as cached_file:
+          data = cached_file.read()
+
+      response += data[max(0, file_begin - position): min(CHUNK_SIZE, file_end - position)]
+
+      position += CHUNK_SIZE
+      if position >= file_end:
+        self._pos = file_end
+        return response
+
+  @retry(wait=wait_random_exponential(multiplier=1, max=5), stop=stop_after_attempt(3), reraise=True)
+  def read_aux(self, ll=None):
+    download_range = False
+    headers = ["Connection: keep-alive"]
+    if self._pos != 0 or ll is not None:
+      if ll is None:
+        end = self.get_length() - 1
+      else:
+        end = min(self._pos + ll, self.get_length()) - 1
+      if self._pos >= end:
+        return b""
+      headers.append(f"Range: bytes={self._pos}-{end}")
+      download_range = True
+
+    dats = BytesIO()
+    c = self._curl
+    c.setopt(pycurl.URL, self._url)
+    c.setopt(pycurl.WRITEDATA, dats)
+    c.setopt(pycurl.NOSIGNAL, 1)
+    c.setopt(pycurl.TIMEOUT_MS, 500000)
+    c.setopt(pycurl.HTTPHEADER, headers)
+    c.setopt(pycurl.FOLLOWLOCATION, True)
+
+    if self._debug:
+      print("downloading", self._url)
+
+      def header(x):
+        if b'MISS' in x:
+          print(x.strip())
+
+      c.setopt(pycurl.HEADERFUNCTION, header)
+
+      def test(debug_type, debug_msg):
+       print("  debug(%d): %s" % (debug_type, debug_msg.strip()))
+
+      c.setopt(pycurl.VERBOSE, 1)
+      c.setopt(pycurl.DEBUGFUNCTION, test)
+      t1 = time.time()
+
+    c.perform()
+
+    if self._debug:
+      t2 = time.time()
+      if t2 - t1 > 0.1:
+        print("get %s %r %.f slow" % (self._url, headers, t2 - t1))
+
+    response_code = c.getinfo(pycurl.RESPONSE_CODE)
+    if response_code == 416:  # Requested Range Not Satisfiable
+      raise Exception(f"Error, range out of bounds {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+    if download_range and response_code != 206:  # Partial Content
+      raise Exception(f"Error, requested range but got unexpected response {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+    if (not download_range) and response_code != 200:  # OK
+      raise Exception(f"Error {response_code} {headers} ({self._url}): {repr(dats.getvalue())[:500]}")
+
+    ret = dats.getvalue()
+    self._pos += len(ret)
+    return ret
+
+  def seek(self, pos):
+    self._pos = pos
+
+  @property
+  def name(self):
+    """Returns a local path to file with the URLFile's contents.
+
+       This can be used to interface with modules that require local files.
+    """
+    if self._local_file is None:
+      _, ext = os.path.splitext(urllib.parse.urlparse(self._url).path)
+      local_fd, local_path = tempfile.mkstemp(suffix=ext)
+      try:
+        os.write(local_fd, self.read())
+        local_file = open(local_path, "rb")
+      except Exception:
+        os.remove(local_path)
+        raise
+      finally:
+        os.close(local_fd)
+
+      self._local_file = local_file
+      self.read = self._local_file.read
+      self.seek = self._local_file.seek
+
+    return self._local_file.name
diff --git a/tools/lib/vidindex/.gitignore b/tools/lib/vidindex/.gitignore
new file mode 100644
index 000000000..a77a06e97
--- /dev/null
+++ b/tools/lib/vidindex/.gitignore
@@ -0,0 +1 @@
+vidindex
diff --git a/tools/lib/vidindex/Makefile b/tools/lib/vidindex/Makefile
new file mode 100644
index 000000000..f6526db21
--- /dev/null
+++ b/tools/lib/vidindex/Makefile
@@ -0,0 +1,6 @@
+CC := gcc
+
+vidindex: bitstream.c bitstream.h vidindex.c
+	$(eval $@_TMP := $(shell mktemp))
+	$(CC) -std=c99 bitstream.c vidindex.c -o $($@_TMP)
+	mv $($@_TMP) $@
diff --git a/tools/lib/vidindex/bitstream.c b/tools/lib/vidindex/bitstream.c
new file mode 100644
index 000000000..d174ffa8a
--- /dev/null
+++ b/tools/lib/vidindex/bitstream.c
@@ -0,0 +1,118 @@
+#include <stdbool.h>
+#include <assert.h>
+
+#include "bitstream.h"
+
+static const uint32_t BS_MASKS[33] = {
+    0,           0x1L,        0x3L,       0x7L,       0xFL,       0x1FL,
+    0x3FL,       0x7FL,       0xFFL,      0x1FFL,     0x3FFL,     0x7FFL,
+    0xFFFL,      0x1FFFL,     0x3FFFL,    0x7FFFL,    0xFFFFL,    0x1FFFFL,
+    0x3FFFFL,    0x7FFFFL,    0xFFFFFL,   0x1FFFFFL,  0x3FFFFFL,  0x7FFFFFL,
+    0xFFFFFFL,   0x1FFFFFFL,  0x3FFFFFFL, 0x7FFFFFFL, 0xFFFFFFFL, 0x1FFFFFFFL,
+    0x3FFFFFFFL, 0x7FFFFFFFL, 0xFFFFFFFFL};
+
+void bs_init(struct bitstream* bs, const uint8_t* buffer, size_t input_size) {
+  bs->buffer_ptr = buffer;
+  bs->buffer_end = buffer + input_size;
+  bs->value = 0;
+  bs->pos = 0;
+  bs->shift = 8;
+  bs->size = input_size * 8;
+}
+
+uint32_t bs_get(struct bitstream* bs, int n) {
+  if (n > 32)
+    return 0;
+
+  bs->pos += n;
+  bs->shift += n;
+  while (bs->shift > 8) {
+    if (bs->buffer_ptr < bs->buffer_end) {
+      bs->value <<= 8;
+      bs->value |= *bs->buffer_ptr++;
+      bs->shift -= 8;
+    } else {
+      bs_seek(bs, bs->pos - n);
+      return 0;
+      // bs->value <<= 8;
+      // bs->shift -= 8;
+    }
+  }
+  return (bs->value >> (8 - bs->shift)) & BS_MASKS[n];
+}
+
+void bs_seek(struct bitstream* bs, size_t new_pos) {
+  bs->pos = (new_pos / 32) * 32;
+  bs->shift = 8;
+  bs->value = 0;
+  bs_get(bs, new_pos % 32);
+}
+
+uint32_t bs_peek(struct bitstream* bs, int n) {
+  struct bitstream bak = *bs;
+  return bs_get(&bak, n);
+}
+
+size_t bs_remain(struct bitstream* bs) {
+  return bs->size - bs->pos;
+}
+
+int bs_eof(struct bitstream* bs) {
+  return bs_remain(bs) == 0;
+}
+
+uint32_t bs_ue(struct bitstream* bs) {
+  static const uint8_t exp_golomb_bits[256] = {
+      8, 7, 6, 6, 5, 5, 5, 5, 4, 4, 4, 4, 4, 4, 4, 4, 3, 3, 3, 3, 3, 3, 3, 3,
+      3, 3, 3, 3, 3, 3, 3, 3, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2,
+      2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 1, 1, 1, 1, 1, 1, 1, 1,
+      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+      1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
+      1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+      0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  };
+  uint32_t bits, read = 0;
+  int bits_left;
+  uint8_t coded;
+  int done = 0;
+  bits = 0;
+  // we want to read 8 bits at a time - if we don't have 8 bits,
+  // read what's left, and shift.  The exp_golomb_bits calc remains the
+  // same.
+  while (!done) {
+    bits_left = bs_remain(bs);
+    if (bits_left < 8) {
+      read = bs_peek(bs, bits_left) << (8 - bits_left);
+      done = 1;
+    } else {
+      read = bs_peek(bs, 8);
+      if (read == 0) {
+        bs_get(bs, 8);
+        bits += 8;
+      } else {
+        done = 1;
+      }
+    }
+  }
+  coded = exp_golomb_bits[read];
+  bs_get(bs, coded);
+  bits += coded;
+
+  //  printf("ue - bits %d\n", bits);
+  return bs_get(bs, bits + 1) - 1;
+}
+
+int32_t bs_se(struct bitstream* bs) {
+  uint32_t ret;
+  ret = bs_ue(bs);
+  if ((ret & 0x1) == 0) {
+    ret >>= 1;
+    int32_t temp = 0 - ret;
+    return temp;
+  }
+  return (ret + 1) >> 1;
+}
diff --git a/tools/lib/vidindex/bitstream.h b/tools/lib/vidindex/bitstream.h
new file mode 100644
index 000000000..0f538a59a
--- /dev/null
+++ b/tools/lib/vidindex/bitstream.h
@@ -0,0 +1,26 @@
+#ifndef bitstream_H
+#define bitstream_H
+
+
+#include <stddef.h>
+#include <stdint.h>
+
+struct bitstream {
+  const uint8_t *buffer_ptr;
+  const uint8_t *buffer_end;
+  uint64_t value;
+  uint32_t pos;
+  uint32_t shift;
+  size_t size;
+};
+
+void bs_init(struct bitstream *bs, const uint8_t *buffer, size_t input_size);
+void bs_seek(struct bitstream *bs, size_t new_pos);
+uint32_t bs_get(struct bitstream *bs, int n);
+uint32_t bs_peek(struct bitstream *bs, int n);
+size_t bs_remain(struct bitstream *bs);
+int bs_eof(struct bitstream *bs);
+uint32_t bs_ue(struct bitstream *bs);
+int32_t bs_se(struct bitstream *bs);
+
+#endif
diff --git a/tools/lib/vidindex/vidindex.c b/tools/lib/vidindex/vidindex.c
new file mode 100644
index 000000000..4857c60dd
--- /dev/null
+++ b/tools/lib/vidindex/vidindex.c
@@ -0,0 +1,307 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+
+#include "bitstream.h"
+
+#define START_CODE 0x000001
+
+static uint32_t read24be(const uint8_t* ptr) {
+    return (ptr[0] << 16) | (ptr[1] << 8) | ptr[2];
+}
+static void write32le(FILE *of, uint32_t v) {
+  uint8_t va[4] = {
+    v & 0xff, (v >> 8) & 0xff, (v >> 16) & 0xff, (v >> 24) & 0xff
+  };
+  fwrite(va, 1, sizeof(va), of);
+}
+
+// Table 7-1
+enum hevc_nal_type {
+  HEVC_NAL_TYPE_TRAIL_N = 0,
+  HEVC_NAL_TYPE_TRAIL_R = 1,
+  HEVC_NAL_TYPE_TSA_N = 2,
+  HEVC_NAL_TYPE_TSA_R = 3,
+  HEVC_NAL_TYPE_STSA_N = 4,
+  HEVC_NAL_TYPE_STSA_R = 5,
+  HEVC_NAL_TYPE_RADL_N = 6,
+  HEVC_NAL_TYPE_RADL_R = 7,
+  HEVC_NAL_TYPE_RASL_N = 8,
+  HEVC_NAL_TYPE_RASL_R = 9,
+  HEVC_NAL_TYPE_BLA_W_LP = 16,
+  HEVC_NAL_TYPE_BLA_W_RADL = 17,
+  HEVC_NAL_TYPE_BLA_N_LP = 18,
+  HEVC_NAL_TYPE_IDR_W_RADL = 19,
+  HEVC_NAL_TYPE_IDR_N_LP = 20,
+  HEVC_NAL_TYPE_CRA_NUT = 21,
+  HEVC_NAL_TYPE_RSV_IRAP_VCL23 = 23,
+  HEVC_NAL_TYPE_VPS_NUT = 32,
+  HEVC_NAL_TYPE_SPS_NUT = 33,
+  HEVC_NAL_TYPE_PPS_NUT = 34,
+  HEVC_NAL_TYPE_AUD_NUT = 35,
+  HEVC_NAL_TYPE_EOS_NUT = 36,
+  HEVC_NAL_TYPE_EOB_NUT = 37,
+  HEVC_NAL_TYPE_FD_NUT = 38,
+  HEVC_NAL_TYPE_PREFIX_SEI_NUT = 39,
+  HEVC_NAL_TYPE_SUFFIX_SEI_NUT = 40,
+};
+
+// Table 7-7
+enum hevc_slice_type {
+  HEVC_SLICE_B = 0,
+  HEVC_SLICE_P = 1,
+  HEVC_SLICE_I = 2,
+};
+
+static void hevc_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
+  const uint8_t* ptr = data;
+  const uint8_t* ptr_end = data + file_size;
+
+  assert(ptr[0] == 0);
+  ptr++;
+  assert(read24be(ptr) == START_CODE);
+
+  // pps. ignore for now
+  uint32_t num_extra_slice_header_bits = 0;
+  uint32_t dependent_slice_segments_enabled_flag = 0;
+
+  while (ptr < ptr_end) {
+    const uint8_t* next = ptr+1;
+    for (; next < ptr_end-4; next++) {
+      if (read24be(next) == START_CODE) break;
+    }
+    size_t nal_size = next - ptr;
+    if (nal_size < 6) {
+      break;
+    }
+
+    {
+      struct bitstream bs = {0};
+      bs_init(&bs, ptr, nal_size);
+
+      uint32_t start_code = bs_get(&bs, 24);
+      assert(start_code == 0x000001);
+
+      // nal_unit_header
+      uint32_t forbidden_zero_bit = bs_get(&bs, 1);
+      uint32_t nal_unit_type = bs_get(&bs, 6);
+      uint32_t nuh_layer_id = bs_get(&bs, 6);
+      uint32_t nuh_temporal_id_plus1 = bs_get(&bs, 3);
+
+      // if (nal_unit_type != 1) printf("%3d -- %3d %10d %lu\n", nal_unit_type, frame_num, (uint32_t)(ptr-data), nal_size);
+
+      switch (nal_unit_type) {
+      case HEVC_NAL_TYPE_VPS_NUT:
+      case HEVC_NAL_TYPE_SPS_NUT:
+      case HEVC_NAL_TYPE_PPS_NUT:
+        fwrite(ptr, 1, nal_size, of_prefix);
+        break;
+      case HEVC_NAL_TYPE_TRAIL_N:
+      case HEVC_NAL_TYPE_TRAIL_R:
+      case HEVC_NAL_TYPE_TSA_N:
+      case HEVC_NAL_TYPE_TSA_R:
+      case HEVC_NAL_TYPE_STSA_N:
+      case HEVC_NAL_TYPE_STSA_R:
+      case HEVC_NAL_TYPE_RADL_N:
+      case HEVC_NAL_TYPE_RADL_R:
+      case HEVC_NAL_TYPE_RASL_N:
+      case HEVC_NAL_TYPE_RASL_R:
+      case HEVC_NAL_TYPE_BLA_W_LP:
+      case HEVC_NAL_TYPE_BLA_W_RADL:
+      case HEVC_NAL_TYPE_BLA_N_LP:
+      case HEVC_NAL_TYPE_IDR_W_RADL:
+      case HEVC_NAL_TYPE_IDR_N_LP:
+      case HEVC_NAL_TYPE_CRA_NUT: {
+        // slice_segment_header
+        uint32_t first_slice_segment_in_pic_flag = bs_get(&bs, 1);
+        if (nal_unit_type >= HEVC_NAL_TYPE_BLA_W_LP && nal_unit_type <= HEVC_NAL_TYPE_RSV_IRAP_VCL23) {
+          uint32_t no_output_of_prior_pics_flag = bs_get(&bs, 1);
+        }
+        uint32_t slice_pic_parameter_set_id = bs_get(&bs, 1);
+        if (!first_slice_segment_in_pic_flag) {
+          // ...
+          break;
+        }
+
+        if (!dependent_slice_segments_enabled_flag) {
+          for (int i=0; i<num_extra_slice_header_bits; i++) {
+            bs_get(&bs, 1);
+          }
+          uint32_t slice_type = bs_ue(&bs);
+
+          // write the index
+          write32le(of_index, slice_type);
+          write32le(of_index, ptr - data);
+
+          // ...
+        }
+
+        break;
+      }
+      }
+
+      //...
+      // emulation_prevention_three_byte
+    }
+
+    ptr = next;
+  }
+
+  write32le(of_index, -1);
+  write32le(of_index, file_size);
+}
+
+// Table 7-1
+enum h264_nal_type {
+  H264_NAL_SLICE           = 1,
+  H264_NAL_DPA             = 2,
+  H264_NAL_DPB             = 3,
+  H264_NAL_DPC             = 4,
+  H264_NAL_IDR_SLICE       = 5,
+  H264_NAL_SEI             = 6,
+  H264_NAL_SPS             = 7,
+  H264_NAL_PPS             = 8,
+  H264_NAL_AUD             = 9,
+  H264_NAL_END_SEQUENCE    = 10,
+  H264_NAL_END_STREAM      = 11,
+  H264_NAL_FILLER_DATA     = 12,
+  H264_NAL_SPS_EXT         = 13,
+  H264_NAL_AUXILIARY_SLICE = 19,
+};
+
+enum h264_slice_type {
+  H264_SLICE_P = 0,
+  H264_SLICE_B = 1,
+  H264_SLICE_I = 2,
+  // ...
+};
+
+static void h264_index(const uint8_t *data, size_t file_size, FILE *of_prefix, FILE *of_index) {
+  const uint8_t* ptr = data;
+  const uint8_t* ptr_end = data + file_size;
+
+  assert(ptr[0] == 0);
+  ptr++;
+  assert(read24be(ptr) == START_CODE);
+
+
+  uint32_t sps_log2_max_frame_num_minus4;
+
+
+  int last_frame_num = -1;
+
+  while (ptr < ptr_end) {
+    const uint8_t* next = ptr+1;
+    for (; next < ptr_end-4; next++) {
+      if (read24be(next) == START_CODE) break;
+    }
+    size_t nal_size = next - ptr;
+    if (nal_size < 5) {
+      break;
+    }
+
+    {
+      struct bitstream bs = {0};
+      bs_init(&bs, ptr, nal_size);
+
+      uint32_t start_code = bs_get(&bs, 24);
+      assert(start_code == 0x000001);
+
+      // nal_unit_header
+      uint32_t forbidden_zero_bit = bs_get(&bs, 1);
+      uint32_t nal_ref_idx = bs_get(&bs, 2);
+      uint32_t nal_unit_type = bs_get(&bs, 5);
+
+      switch (nal_unit_type) {
+      case H264_NAL_SPS:
+
+        {
+          uint32_t profile_idx = bs_get(&bs, 8);
+          uint32_t constraint_sets = bs_get(&bs, 4);
+          uint32_t reserved = bs_get(&bs, 5);
+          uint32_t level_idc = bs_get(&bs, 5);
+          uint32_t seq_parameter_set_id = bs_ue(&bs);
+          sps_log2_max_frame_num_minus4 = bs_ue(&bs);
+        }
+
+        // fallthrough
+      case H264_NAL_PPS:
+        fwrite(ptr, 1, nal_size, of_prefix);
+        break;
+
+      case H264_NAL_SLICE:
+      case H264_NAL_IDR_SLICE: {
+        // slice header
+        uint32_t first_mb_in_slice = bs_ue(&bs);
+        uint32_t slice_type = bs_ue(&bs);
+        uint32_t pic_parameter_set_id = bs_ue(&bs);
+
+        uint32_t frame_num = bs_get(&bs, sps_log2_max_frame_num_minus4+4);
+
+        if (first_mb_in_slice == 0) {
+          write32le(of_index, slice_type);
+          write32le(of_index, ptr - data);
+        }
+
+        break;
+      }
+
+      }
+
+    }
+
+    ptr = next;
+  }
+
+  write32le(of_index, -1);
+  write32le(of_index, file_size);
+}
+
+int main(int argc, char** argv) {
+  if (argc != 5) {
+    fprintf(stderr, "usage: %s h264|hevc file_path out_prefix out_index\n", argv[0]);
+    exit(1);
+  }
+
+  const char* file_type = argv[1];
+  const char* file_path = argv[2];
+
+  int fd = open(file_path, O_RDONLY, 0);
+  if (fd < 0) {
+    fprintf(stderr, "error: couldn't open %s\n", file_path);
+    exit(1);
+  }
+
+  FILE *of_prefix = fopen(argv[3], "wb");
+  assert(of_prefix);
+  FILE *of_index = fopen(argv[4], "wb");
+  assert(of_index);
+
+  off_t file_size = lseek(fd, 0, SEEK_END);
+  lseek(fd, 0, SEEK_SET);
+
+  assert(file_size > 4);
+
+  const uint8_t* data = (const uint8_t*)mmap(NULL, file_size, PROT_READ, MAP_PRIVATE, fd, 0);
+  assert(data != MAP_FAILED);
+
+  if (strcmp(file_type, "hevc") == 0) {
+    hevc_index(data, file_size, of_prefix, of_index);
+  } else if (strcmp(file_type, "h264") == 0) {
+    h264_index(data, file_size, of_prefix, of_index);
+  } else {
+    assert(false);
+  }
+
+  munmap((void*)data, file_size);
+  close(fd);
+
+  return 0;
+}