diff --git a/cereal/Makefile b/cereal/Makefile
index dc6b7f9d51fa03..e13b059f633059 100644
--- a/cereal/Makefile
+++ b/cereal/Makefile
@@ -1,14 +1,14 @@
 PWD := $(shell pwd)
 
-SRCS := log.capnp car.capnp
+SRCS := log.capnp car.capnp ui.capnp
 
-GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++
-JS := gen/js/car.capnp.js gen/js/log.capnp.js
+GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/ui.capnp.c++
+JS := gen/js/car.capnp.js gen/js/log.capnp.js gen/js/ui.capnp.js
 
 UNAME_M ?= $(shell uname -m)
 # only generate C++ for docker tests
 ifneq ($(OPTEST),1)
-	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
+	GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/ui.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h
 
 	ifeq ($(UNAME_M),x86_64)
 		ifneq (, $(shell which capnpc-java))
diff --git a/cereal/__init__.py b/cereal/__init__.py
index 2d3b48526b2c34..2638b0c07520fd 100644
--- a/cereal/__init__.py
+++ b/cereal/__init__.py
@@ -6,3 +6,4 @@
 
 log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp"))
 car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp"))
+ui = capnp.load(os.path.join(CEREAL_PATH, "ui.capnp"))
diff --git a/cereal/car.capnp b/cereal/car.capnp
index bf982f8aba0da6..58a834b8b0670e 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -116,6 +116,13 @@ struct CarState {
   leftBlinker @20 :Bool;
   rightBlinker @21 :Bool;
   genericToggle @23 :Bool;
+  blindspot @26 :Bool;
+  distanceToggle @27 :Float32;
+  laneDepartureToggle @28 :Bool;
+  accSlowToggle @29 :Bool;
+  blindspotside @30 :Float32;
+  readdistancelines @31 :Float32;
+  gasbuttonstatus @32 :Float32;
 
   # lock info
   doorOpen @24 :Bool;
diff --git a/cereal/ui.capnp b/cereal/ui.capnp
new file mode 100644
index 00000000000000..82db24287d30ee
--- /dev/null
+++ b/cereal/ui.capnp
@@ -0,0 +1,64 @@
+using Cxx = import "./include/c++.capnp";
+$Cxx.namespace("cereal");
+
+using Java = import "./include/java.capnp";
+$Java.package("ai.comma.openpilot.cereal");
+$Java.outerClassname("Ui");
+
+using Car = import "car.capnp";
+
+@0xce6ca45dddcd5317;
+
+struct UIButtonInfo {
+    # button ID 0..5
+    btnId @0 :Int8;
+    # internal button name
+    btnName @1 :Text;
+    # display label for button (3 chars)
+    btnLabel @2 :Text;
+    # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE
+    btnStatus @3 :Int16;
+    # small font label shows below the main label, max 7 chars
+    btnLabel2 @4 :Text;
+}
+
+struct UIButtonStatus {
+    # button ID 0..5
+    btnId @0 :Int8;
+    # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE
+    btnStatus @1 :Int16;
+}
+
+struct UICustomAlert {
+    caStatus @0 :Int8;
+    caText @1 :Text;
+}
+
+struct UISetCar {
+    icCarFolder @0 :Text;
+    icCarName @1 :Text;
+}
+
+struct UIPlaySound {
+    sndSound @0 :Int8;
+}
+
+struct UIUpdate {
+    uiDoUpdate @0 :Int8;
+    uiStatus @1 :Int8;
+    uiCanDisplayMessage @2 :Int8;
+}
+
+#struct UIEvent {
+#    # in nanoseconds?
+#    logMonoTime @0 :UInt64;
+#
+#    union {
+#        uiButtonInfo @1 :UIButtonInfo;
+#        uiCustomAlert @2 :UICustomAlert;
+#        uiSetCar @3 :UISetCar;
+#        uiButtonStatus @4 :UIButtonStatus;
+#        uiUpdate @5 :UIUpdate;
+#        uiPlaySound @6 :UIPlaySound;
+#    }
+#}
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index 1cb9830e39f26e..f09b6ab81cbd0a 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -70,7 +70,7 @@ def process_hud_alert(hud_alert):
 
 HUDData = namedtuple("HUDData",
                      ["pcm_accel", "v_cruise", "mini_car", "car", "X4",
-                      "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"])
+                      "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required", "dist_lines"])
 
 
 class CarController(object):
@@ -118,7 +118,12 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
         hud_car = 1
     else:
       hud_car = 0
-
+       
+    #if CS.readdistancelines == 0 or CS.readdistancelines == 1 or CS.readdistancelines == 2 or CS.readdistancelines == 3:
+    #  distance_lines = CS.readdistancelines
+    #else:
+    #  distance_lines = 1
+      
     # For lateral control-only, send chimes as a beep since we don't send 0x1fa
     if CS.CP.radarOffCan:
       snd_beep = snd_beep if snd_beep is not 0 else snd_chime
@@ -127,7 +132,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \
     fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
 
     hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car,
-                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required)
+                  0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines)
 
     # **** process the car messages ****
 
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 99d4a2214300c2..7523b32a193784 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -143,6 +143,9 @@ def get_cam_can_parser(CP):
 
 class CarState(object):
   def __init__(self, CP):
+    self.trLabels = ["0.9","1.8","2.7"]
+    self.trMode = 1
+    self.read_distance_lines_prev = 3
     self.CP = CP
     self.can_define = CANDefine(DBC[CP.carFingerprint]['pt'])
     self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"]
@@ -183,7 +186,7 @@ def update(self, cp, cp_cam):
 
     # update prevs, update must run once per loop
     self.prev_cruise_buttons = self.cruise_buttons
-    self.prev_cruise_setting = self.cruise_setting
+    #self.prev_cruise_setting = self.cruise_setting
     self.prev_blinker_on = self.blinker_on
 
     self.prev_left_blinker_on = self.left_blinker_on
@@ -242,7 +245,7 @@ def update(self, cp, cp_cam):
     self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
     self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
 
-    self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
+    #self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
     self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']
 
     self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER']
@@ -303,6 +306,21 @@ def update(self, cp, cp_cam):
     self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE']
     self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS']
     self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD']
+    
+    if self.cruise_setting == 3:
+      if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0:
+        self.trMode = (self.trMode + 1 ) % 3
+    self.prev_cruise_setting = self.cruise_setting
+    self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
+    self.read_distance_lines = self.trMode + 1
+    if self.read_distance_lines <> self.read_distance_lines_prev:
+    #  if self.read_distance_lines == 1:
+    #    self.UE.custom_alert_message(2,"Following distance set to 0.9s",200,3)
+    #  if self.read_distance_lines == 2:
+    #    self.UE.custom_alert_message(2,"Following distance set to 1.8s",200,3)
+    #  if self.read_distance_lines == 3:
+    #    self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3)
+      self.read_distance_lines_prev = self.read_distance_lines
 
 
 # carstate standalone tester
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 3d92d24a7c2924..58178acb39edef 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -77,9 +77,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx):
       'CRUISE_SPEED': hud.v_cruise,
       'ENABLE_MINI_CAR': hud.mini_car,
       'HUD_LEAD': hud.car,
-      'SET_ME_X03': 0x03,
+      'SET_ME_X03': hud.dist_lines,
       'SET_ME_X03_2': 0x03,
       'SET_ME_X01': 0x01,
+      'HUD_DISTANCE_3': 1,
     }
     commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx))
 
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 6a4a007e0c2c79..3e2cf76efaebd1 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -432,6 +432,8 @@ def update(self, c):
     ret.cruiseState.available = bool(self.CS.main_on)
     ret.cruiseState.speedOffset = self.CS.cruise_speed_offset
     ret.cruiseState.standstill = False
+    
+    ret.readdistancelines = self.CS.read_distance_lines
 
     # TODO: button presses
     buttonEvents = []
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9d78c966743569..1faefc4dd08f31 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -18,7 +18,7 @@ class MPC_COST_LAT:
 
 class MPC_COST_LONG:
   TTC = 5.0
-  DISTANCE = 0.1
+  DISTANCE = 0.8
   ACCELERATION = 10.0
   JERK = 20.0
 
diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile
index 17f78eada990df..93cb0a880ef403 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/Makefile
+++ b/selfdrive/controls/lib/longitudinal_mpc/Makefile
@@ -72,7 +72,7 @@ lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp
            -c -o '$@' '$<'
 
 generator: generator.cpp
-	$(CXX) -Wall -std=c++11 \
+	$(CXX) -v -Wall -std=c++11 \
          generator.cpp \
          -o generator \
          $(ACADO_FLAGS) \
diff --git a/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh b/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh
new file mode 100644
index 00000000000000..2098f2ab08851a
--- /dev/null
+++ b/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh
@@ -0,0 +1,2 @@
+#!/usr/bin/env bash
+sed -i -e 's/1\.2345678[^)]*)/TR)/g' -e 's/out)/out, double TR)/g' -e 's/Objective(  )/Objective(double TR)/g' -e 's/ValueOut )/ValueOut, TR )/g' -e 's/ionStep(  )/ionStep(double TR)/g' -e 's/Objective(double TR);/Objective(TR);/g' mpc_export/acado_solver.c
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
index 676787bbfcbc7b..8bd1970888e5ab 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
@@ -104,7 +104,7 @@ acadoWorkspace.evGu[lRun1 * 6 + 5] = acadoWorkspace.state[47];
 return ret;
 }
 
-void acado_evaluateLSQ(const real_t* in, real_t* out)
+void acado_evaluateLSQ(const real_t* in, real_t* out, double TR)
 {
 const real_t* xd = in;
 const real_t* u = in + 6;
@@ -113,31 +113,31 @@ 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))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
 a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
+a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
 a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
 a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]);
 a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
 a[9] = (a[8]*(real_t)(5.0000000000000000e-01));
 a[10] = (a[4]*a[4]);
-a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
+a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
 a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01)));
 a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
 a[16] = (a[15]*(real_t)(5.0000000000000000e-01));
 a[17] = (a[14]*a[14]);
-a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
+a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
+a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[12]))-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
 a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]);
 a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
+a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
 a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
+a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
 a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
 a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[28] = (a[26]*a[26]);
@@ -145,7 +145,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 
 /* Compute outputs: */
 out[0] = (a[1]-a[3]);
-out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
+out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(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[6];
@@ -155,10 +155,10 @@ out[7] = a[20];
 out[8] = (a[22]-a[25]);
 out[9] = (real_t)(0.0000000000000000e+00);
 out[10] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]);
-out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
+out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
 out[12] = (real_t)(0.0000000000000000e+00);
 out[13] = a[26];
-out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]);
+out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[29])))*a[26]);
 out[15] = (real_t)(0.0000000000000000e+00);
 out[16] = (real_t)(0.0000000000000000e+00);
 out[17] = (xd[2]*(real_t)(1.0000000000000001e-01));
@@ -178,7 +178,7 @@ out[30] = (real_t)(0.0000000000000000e+00);
 out[31] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
 }
 
-void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
+void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR)
 {
 const real_t* xd = in;
 /* Vector of auxiliary variables; number of elements: 30. */
@@ -186,31 +186,31 @@ 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))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
 a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
+a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
 a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
 a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]);
 a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
 a[9] = (a[8]*(real_t)(5.0000000000000000e-01));
 a[10] = (a[4]*a[4]);
-a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
+a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
 a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01)));
 a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
 a[16] = (a[15]*(real_t)(5.0000000000000000e-01));
 a[17] = (a[14]*a[14]);
-a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
+a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
+a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[12]))-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
 a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]);
 a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
+a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
 a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
+a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
 a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
 a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 a[28] = (a[26]*a[26]);
@@ -218,7 +218,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
 
 /* Compute outputs: */
 out[0] = (a[1]-a[3]);
-out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
+out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(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[6];
 out[4] = (a[11]-a[19]);
@@ -227,10 +227,10 @@ out[6] = a[20];
 out[7] = (a[22]-a[25]);
 out[8] = (real_t)(0.0000000000000000e+00);
 out[9] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]);
-out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
+out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
 out[11] = (real_t)(0.0000000000000000e+00);
 out[12] = a[26];
-out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]);
+out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[29])))*a[26]);
 out[14] = (real_t)(0.0000000000000000e+00);
 out[15] = (real_t)(0.0000000000000000e+00);
 out[16] = (xd[2]*(real_t)(1.0000000000000001e-01));
@@ -371,7 +371,7 @@ tmpQN1[34] = + tmpQN2[15]*tmpFx[4] + tmpQN2[16]*tmpFx[10] + tmpQN2[17]*tmpFx[16]
 tmpQN1[35] = + tmpQN2[15]*tmpFx[5] + tmpQN2[16]*tmpFx[11] + tmpQN2[17]*tmpFx[17];
 }
 
-void acado_evaluateObjective(  )
+void acado_evaluateObjective(double TR)
 {
 int runObj;
 for (runObj = 0; runObj < 20; ++runObj)
@@ -386,7 +386,7 @@ acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj];
 acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2];
 acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1];
 
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
+acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR);
 acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
 acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1];
 acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2];
@@ -405,7 +405,7 @@ acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
 acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
 acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
+acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
 
 acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
 acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1];
@@ -5158,12 +5158,12 @@ acado_multEDu( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVa
 acado_multEDu( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.x[ 25 ]), &(acadoVariables.x[ 120 ]) );
 }
 
-int acado_preparationStep(  )
+int acado_preparationStep(double TR)
 {
 int ret;
 
 ret = acado_modelSimulation();
-acado_evaluateObjective(  );
+acado_evaluateObjective(TR);
 acado_condensePrep(  );
 return ret;
 }
@@ -5313,7 +5313,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd);
 return kkt;
 }
 
-real_t acado_getObjective(  )
+real_t acado_getObjective(double TR)
 {
 real_t objVal;
 
@@ -5336,7 +5336,7 @@ acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1];
 acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2];
 acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1];
 
-acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
+acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
 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];
@@ -5350,7 +5350,7 @@ acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
 acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
 acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
 acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
-acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
+acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR );
 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];
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
index 8f2fcfbdf156ea..b53d2fe76f5a64 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
index 232e9a158b206c..a5baf2163eb1df 100755
Binary files a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so and b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 36b6f8b7e0b8d3..02d7600a35791c 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -39,7 +39,7 @@ def _get_libmpc(mpc_id):
     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);
+                double l, double a_l_0, double TR);
     """)
 
     return (ffi, ffi.dlopen(libmpc_fn))
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
index c0b43e53c93771..9e05bfa9e01ada 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
@@ -118,7 +118,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_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){
+int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){
   int i;
 
   for (i = 0; i <= NOD * N; i+= NOD){
@@ -133,7 +133,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
   acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
   acadoVariables.x[5] = acadoVariables.x0[5] = 0.0;
 
-  acado_preparationStep();
+  acado_preparationStep(TR);
   acado_feedbackStep();
 
 	for (i = 0; i <= N; i++){
@@ -146,7 +146,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
 
     solution->j_ego[i] = acadoVariables.u[i];
 	}
-  solution->cost = acado_getObjective();
+  solution->cost = acado_getObjective(TR);
 
   // Dont shift states here. Current solution is closer to next timestep than if
   // we shift by 0.2 seconds.
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
index e899d7b785a16f..479d21492ac8fd 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o and b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o differ
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index b986740113904d..2a979cf83b7705 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -28,6 +28,7 @@
 _DT_MPC = 0.2  # 5Hz
 MAX_SPEED_ERROR = 2.0
 AWARENESS_DECEL = -0.2     # car smoothly decel at .2m/s^2 when user is distracted
+TR=1.8 # CS.readdistancelines
 
 GPS_PLANNER_ADDR = "192.168.5.1"
 
@@ -151,7 +152,8 @@ def __init__(self, mpc_id, live_longitudinal_mpc):
     self.prev_lead_status = False
     self.prev_lead_x = 0.0
     self.new_lead = False
-
+    
+    self.lastTR = 2
     self.last_cloudlog_t = 0.0
 
   def send_mpc_solution(self, qp_iterations, calculation_time):
@@ -219,7 +221,39 @@ def update(self, CS, lead, v_cruise_setpoint):
 
     # Calculate mpc
     t = sec_since_boot()
-    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+    
+    if CS.vEgo < 11.4:
+      TR=1.8 # under 41km/hr use a TR of 1.8 seconds
+      #if self.lastTR > 0:
+        #self.libmpc.init(MPC_COST_LONG.TTC, 0.1, PC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+        #self.lastTR = 0
+    else:
+      if CS.readdistancelines == 2:
+        if CS.readdistancelines == self.lastTR:
+          TR=1.8 # 20m at 40km/hr
+        else:
+          TR=1.8
+          self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          self.lastTR = CS.readdistancelines
+      elif CS.readdistancelines == 1:
+        if CS.readdistancelines == self.lastTR:
+          TR=0.9 # 10m at 40km/hr
+        else:
+          TR=0.9
+          self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          self.lastTR = CS.readdistancelines
+      elif CS.readdistancelines == 3:
+        if CS.readdistancelines == self.lastTR:
+          TR=2.7
+        else:
+          TR=2.7 # 30m at 40km/hr
+          self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+          self.lastTR = CS.readdistancelines
+      else:
+        TR=1.8 # if readdistancelines = 0
+    #print TR
+    
+    n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR)
     duration = int((sec_since_boot() - t) * 1e9)
     self.send_mpc_solution(n_its, duration)