diff --git a/SConstruct b/SConstruct
index 02a93eb98d..efdfb52693 100644
--- a/SConstruct
+++ b/SConstruct
@@ -67,10 +67,17 @@ USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
 
 lenv = {
   "PATH": os.environ['PATH'],
+  "LD_LIBRARY_PATH": [Dir(f"#phonelibs/acados/{arch}/lib").abspath],
+  "PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath,
+
+  "ACADOS_SOURCE_DIR": Dir("#phonelibs/acados/acados").abspath,
+  "TERA_PATH": Dir("#").abspath + f"/phonelibs/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
@@ -87,6 +94,7 @@ if arch == "aarch64" or arch == "larch64":
     "/system/vendor/lib64",
     "/system/comma/usr/lib",
     "#phonelibs/nanovg",
+    f"#phonelibs/acados/{arch}/lib",
   ]
 
   if arch == "larch64":
@@ -100,8 +108,9 @@ 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",
@@ -109,7 +118,6 @@ if arch == "aarch64" or arch == "larch64":
     ]
     cflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
     cxxflags = ["-DQCOM", "-D_USING_LIBCXX", "-mcpu=cortex-a57"]
-    rpath = []
 else:
   cflags = []
   cxxflags = []
@@ -134,6 +142,7 @@ else:
     ]
   else:
     libpath = [
+      "#phonelibs/acados/x86_64/lib",
       "#phonelibs/snpe/x86_64-linux-clang",
       "#phonelibs/libyuv/x64/lib",
       "#phonelibs/mapbox-gl-native-qt/x86_64",
@@ -143,15 +152,12 @@ else:
       "/usr/local/lib",
     ]
 
-  rpath = [
-    "phonelibs/snpe/x86_64-linux-clang",
-    "cereal",
-    "selfdrive/common"
+  rpath += [
+    Dir("#phonelibs/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 +170,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,6 +194,9 @@ env = Environment(
 
   CPPPATH=cpppath + [
     "#",
+    "#phonelibs/acados/include",
+    "#phonelibs/acados/include/blasfeo/include",
+    "#phonelibs/acados/include/hpipm/include",
     "#phonelibs/catch2/include",
     "#phonelibs/bzip2",
     "#phonelibs/libyuv/include",
@@ -410,7 +416,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/lateral_mpc_lib/SConscript'])
 SConscript(['selfdrive/controls/lib/lead_mpc_lib/SConscript'])
 SConscript(['selfdrive/controls/lib/longitudinal_mpc_lib/SConscript'])
 
diff --git a/release/files_common b/release/files_common
index 061d1ad57c..a720feef82 100644
--- a/release/files_common
+++ b/release/files_common
@@ -237,33 +237,16 @@ 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/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/lateral_mpc_lib/.gitignore
 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/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/lead_mpc_lib/*
+selfdrive/controls/lib/longitudinal_mpc_lib/*
 
 selfdrive/hardware/__init__.py
 selfdrive/hardware/base.h
@@ -481,6 +464,11 @@ phonelibs/snpe/aarch64**
 phonelibs/snpe/larch64**
 phonelibs/snpe/dsp**
 
+phonelibs/acados/x86_64/**
+phonelibs/acados/aarch64/**
+phonelibs/acados/larch64/**
+phonelibs/acados/include/**
+
 phonelibs/android_frameworks_native/**
 phonelibs/android_hardware_libhardware/**
 phonelibs/android_system_core/**
@@ -489,6 +477,7 @@ scripts/update_now.sh
 scripts/stop_updater.sh
 
 pyextra/.gitignore
+pyextra/acados_template/**
 
 rednose/**
 
diff --git a/selfdrive/controls/lib/lateral_mpc_lib/.gitignore b/selfdrive/controls/lib/lateral_mpc_lib/.gitignore
new file mode 100644
index 0000000000..aff2eb082b
--- /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 0000000000..4ebb5cd4e0
--- /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_lib/__init__.py b/selfdrive/controls/lib/lateral_mpc_lib/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
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 0000000000..b36c37ade9
--- /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"
+
+
+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()
+
+  N = 16
+  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 = 10
+
+  # 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(6)):
+    self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
+    self.reset(x0)
+
+  def reset(self, x0=np.zeros(6)):
+    self.x_sol = np.zeros((N+1, 4))
+    self.u_sol = np.zeros((N))
+    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(6))
+    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 = np.column_stack([y_pts, heading_pts*(v_ego+5.0), np.zeros(N+1)])
+    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.x_sol = self.solver.get_slice(0, N+1, 'x')
+    self.u_sol = self.solver.get_slice(0, N, 'u')
+    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 0dab9503b9..7760e4f86a 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()
 
+  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
@@ -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)
diff --git a/selfdrive/controls/tests/test_lateral_mpc.py b/selfdrive/controls/tests/test_lateral_mpc.py
index 727384df1a..ecd8b46598 100644
--- a/selfdrive/controls/tests/test_lateral_mpc.py
+++ b/selfdrive/controls/tests/test_lateral_mpc.py
@@ -1,102 +1,75 @@
 import unittest
 import numpy as np
-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 LAT_MPC_N, CAR_ROTATION_RADIUS
 
 
 def run_mpc(v_ref=30., x_init=0., y_init=0., psi_init=0., curvature_init=0.,
             lane_width=3.6, poly_shift=0.):
 
-  libmpc = libmpc_py.libmpc
-  libmpc.init()
-  libmpc.set_weights(1., 1., 1.)
-
-
-  mpc_solution = libmpc_py.ffi.new("log_t *")
+  lat_mpc = LateralMpc()
+  lat_mpc.set_weights(1., 1., 1.)
 
   y_pts = poly_shift * np.ones(LAT_MPC_N + 1)
   heading_pts = np.zeros(LAT_MPC_N + 1)
 
-
-  cur_state = libmpc_py.ffi.new("state_t *")
-  cur_state.x = x_init
-  cur_state.y = y_init
-  cur_state.psi = psi_init
-  cur_state.curvature = curvature_init
+  x0 = np.array([x_init, y_init, psi_init, curvature_init, v_ref, CAR_ROTATION_RADIUS])
 
   # converge in no more than 20 iterations
   for _ in range(20):
-    libmpc.run_mpc(cur_state, mpc_solution, v_ref,
-                   CAR_ROTATION_RADIUS,
-                   list(y_pts), list(heading_pts))
-
-  return mpc_solution
+    lat_mpc.run(x0, v_ref,
+                CAR_ROTATION_RADIUS,
+                y_pts, heading_pts)
+  return lat_mpc.x_sol
 
 
 class TestLateralMpc(unittest.TestCase):
 
   def _assert_null(self, sol, curvature=1e-6):
-    for i in range(len(sol[0].y)):
-      self.assertAlmostEqual(sol[0].y[i], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0].psi[i], 0., delta=curvature)
-      self.assertAlmostEqual(sol[0].curvature[i], 0., delta=curvature)
+    for i in range(len(sol)):
+      self.assertAlmostEqual(sol[0,i,1], 0., delta=curvature)
+      self.assertAlmostEqual(sol[0,i,2], 0., delta=curvature)
+      self.assertAlmostEqual(sol[0,i,3], 0., delta=curvature)
 
   def _assert_simmetry(self, sol, curvature=1e-6):
-    for i in range(len(sol[0][0].y)):
-      self.assertAlmostEqual(sol[0][0].y[i], -sol[1][0].y[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].psi[i], -sol[1][0].psi[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].curvature[i], -sol[1][0].curvature[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature)
-
-  def _assert_identity(self, sol, ignore_y=False, curvature=1e-6):
-    for i in range(len(sol[0][0].y)):
-      self.assertAlmostEqual(sol[0][0].psi[i], sol[1][0].psi[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].curvature[i], sol[1][0].curvature[i], delta=curvature)
-      self.assertAlmostEqual(sol[0][0].x[i], sol[1][0].x[i], delta=curvature)
-      if not ignore_y:
-        self.assertAlmostEqual(sol[0][0].y[i], sol[1][0].y[i], delta=curvature)
+    for i in range(len(sol)):
+      self.assertAlmostEqual(sol[0,i,1], -sol[1,i,1], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,2], -sol[1,i,2], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,3], -sol[1,i,3], delta=curvature)
+      self.assertAlmostEqual(sol[0,i,0], sol[1,i,0], delta=curvature)
 
   def test_straight(self):
     sol = run_mpc()
-    self._assert_null(sol)
+    self._assert_null(np.array([sol]))
 
   def test_y_symmetry(self):
     sol = []
     for y_init in [-0.5, 0.5]:
       sol.append(run_mpc(y_init=y_init))
-    self._assert_simmetry(sol)
+    self._assert_simmetry(np.array(sol))
 
   def test_poly_symmetry(self):
     sol = []
     for poly_shift in [-1., 1.]:
       sol.append(run_mpc(poly_shift=poly_shift))
-    self._assert_simmetry(sol)
+    self._assert_simmetry(np.array(sol))
 
   def test_curvature_symmetry(self):
     sol = []
     for curvature_init in [-0.1, 0.1]:
       sol.append(run_mpc(curvature_init=curvature_init))
-    self._assert_simmetry(sol)
+    self._assert_simmetry(np.array(sol))
 
   def test_psi_symmetry(self):
     sol = []
     for psi_init in [-0.1, 0.1]:
       sol.append(run_mpc(psi_init=psi_init))
-    self._assert_simmetry(sol)
-
-  def test_y_shift_vs_poly_shift(self):
-    shift = 1.
-    sol = []
-    sol.append(run_mpc(y_init=shift))
-    sol.append(run_mpc(poly_shift=-shift))
-    # need larger curvature than standard, otherwise it false triggers.
-    # this is acceptable because the 2 cases are very different from the optimizer standpoint
-    self._assert_identity(sol, ignore_y=True, curvature=1e-5)
+    self._assert_simmetry(np.array(sol))
 
   def test_no_overshoot(self):
     y_init = 1.
     sol = run_mpc(y_init=y_init)
-    for y in list(sol[0].y):
+    for y in list(sol[:,1]):
       self.assertGreaterEqual(y_init, abs(y))
 
 
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 7cc526af0a..4979116ff9 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-0ada8ee251248f041ed8092abd9b5368f57bbf6e
\ No newline at end of file
+a256ade033538eeead4f34ad7b12be25237ae443
\ No newline at end of file
diff --git a/selfdrive/test/test_onroad.py b/selfdrive/test/test_onroad.py
index 342f28c2c5..9eaa528390 100755
--- a/selfdrive/test/test_onroad.py
+++ b/selfdrive/test/test_onroad.py
@@ -23,7 +23,7 @@ PROCS = {
   "selfdrive.controls.controlsd": 50.0,
   "./loggerd": 45.0,
   "./locationd": 9.1,
-  "selfdrive.controls.plannerd": 20.0,
+  "selfdrive.controls.plannerd": 27.0,
   "./_ui": 15.0,
   "selfdrive.locationd.paramsd": 9.1,
   "./camerad": 7.07,
@@ -53,9 +53,9 @@ if TICI:
   PROCS.update({
     "./loggerd": 60.0,
     "selfdrive.controls.controlsd": 28.0,
+    "selfdrive.controls.plannerd": 15.0,
     "./camerad": 31.0,
     "./_ui": 21.0,
-    "selfdrive.controls.plannerd": 12.0,
     "selfdrive.locationd.paramsd": 5.0,
     "./_dmonitoringmodeld": 10.0,
     "selfdrive.thermald.thermald": 1.5,
diff --git a/tools/sim/Dockerfile.sim b/tools/sim/Dockerfile.sim
index a293d87a9d..faa573f41e 100644
--- a/tools/sim/Dockerfile.sim
+++ b/tools/sim/Dockerfile.sim
@@ -50,6 +50,7 @@ RUN mkdir -p $HOME/openpilot
 COPY SConstruct $HOME/openpilot/
 
 COPY ./phonelibs $HOME/openpilot/phonelibs
+COPY ./pyextra $HOME/openpilot/pyextra
 COPY ./site_scons $HOME/openpilot/site_scons
 COPY ./rednose $HOME/openpilot/rednose
 COPY ./common $HOME/openpilot/common