diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index abd47a7836..9946b40f07 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,10 +1,10 @@
 #pragma once
 
-#define MODEL_PATH_DISTANCE 192
-#define TRAJECTORY_SIZE 33
-#define MIN_DRAW_DISTANCE 10.0
-#define MAX_DRAW_DISTANCE 100.0
-#define POLYFIT_DEGREE 4
-#define SPEED_PERCENTILES 10
-#define DESIRE_PRED_SIZE 32
-#define OTHER_META_SIZE 4
+constexpr int MODEL_PATH_DISTANCE = 192;
+constexpr int  TRAJECTORY_SIZE = 33;
+constexpr float MIN_DRAW_DISTANCE = 10.0;
+constexpr float MAX_DRAW_DISTANCE = 100.0;
+constexpr int POLYFIT_DEGREE = 4;
+constexpr int SPEED_PERCENTILES = 10;
+constexpr int DESIRE_PRED_SIZE = 32;
+constexpr int OTHER_META_SIZE = 4;
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 9a28a070f1..cc6ce5c481 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -117,11 +117,10 @@ int main(int argc, char **argv) {
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
   cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
-  cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
 
   // init the models
   ModelState model;
-  model_init(&model, device_id, context, true);
+  model_init(&model, device_id, context);
   LOGW("models loaded, modeld starting");
 
   // loop
@@ -185,8 +184,8 @@ int main(int argc, char **argv) {
         visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE);
 
         ModelDataRaw model_buf =
-            model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height,
-                             model_transform, NULL, vec_desire);
+            model_eval_frame(&model, yuv_ion.buf_cl, buf_info.width, buf_info.height,
+                             model_transform, vec_desire);
         mt2 = millis_since_boot();
         float model_execution_time = (mt2 - mt1) / 1000.0;
 
@@ -196,10 +195,9 @@ int main(int argc, char **argv) {
         if (run_count < 10) frames_dropped = 0;  // let frame drops warm up
         float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-        const float* raw_pred_ptr = send_raw_pred ? (const float *)model.output : nullptr;
-        model_publish(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
-        model_publish_v2(pm, extra.frame_id, frame_id,  vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
-        posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof);
+        const float *raw_pred_ptr = send_raw_pred ? &model.output[0] : nullptr;
+        model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
+        posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
 
         LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);
         last = mt1;
@@ -217,8 +215,6 @@ int main(int argc, char **argv) {
   err = pthread_join(live_thread_handle, NULL);
   assert(err == 0);
   CL_CHECK(clReleaseContext(context));
-  CL_CHECK(clReleaseCommandQueue(q));
-
   pthread_mutex_destroy(&transform_lock);
   return 0;
 }
diff --git a/selfdrive/modeld/models/commonmodel.cc b/selfdrive/modeld/models/commonmodel.cc
index b16f261c48..cc7d4b157e 100644
--- a/selfdrive/modeld/models/commonmodel.cc
+++ b/selfdrive/modeld/models/commonmodel.cc
@@ -25,7 +25,7 @@ void frame_init(ModelFrame* frame, int width, int height,
 
 float *frame_prepare(ModelFrame* frame, cl_command_queue q,
                            cl_mem yuv_cl, int width, int height,
-                           mat3 transform) {
+                           const mat3 &transform) {
   transform_queue(&frame->transform, q,
                   yuv_cl, width, height,
                   frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
diff --git a/selfdrive/modeld/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h
index dd3bf7a7fa..1e1200db6d 100644
--- a/selfdrive/modeld/models/commonmodel.h
+++ b/selfdrive/modeld/models/commonmodel.h
@@ -1,6 +1,4 @@
-#ifndef COMMONMODEL_H
-#define COMMONMODEL_H
-
+#pragma once
 #define CL_USE_DEPRECATED_OPENCL_1_2_APIS
 #ifdef __APPLE__
 #include <OpenCL/cl.h>
@@ -14,10 +12,6 @@
 #include "transforms/transform.h"
 #include "transforms/loadyuv.h"
 
-#ifdef __cplusplus
-extern "C" {
-#endif
-
 const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
 
 void softmax(const float* input, float* output, size_t len);
@@ -37,12 +31,5 @@ void frame_init(ModelFrame* frame, int width, int height,
                       cl_device_id device_id, cl_context context);
 float *frame_prepare(ModelFrame* frame, cl_command_queue q,
                            cl_mem yuv_cl, int width, int height,
-                           mat3 transform);
+                           const mat3 &transform);
 void frame_free(ModelFrame* frame);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif
-
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index b435fcf31e..ae4b7f9e90 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -8,25 +8,42 @@
 #include "common/timing.h"
 #include "common/params.h"
 #include "driving.h"
-
-#define MIN_VALID_LEN 10.0
-#define TRAJECTORY_SIZE 33
-#define TRAJECTORY_TIME 10.0
-#define TRAJECTORY_DISTANCE 192.0
-#define PLAN_IDX 0
-#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE)
-#define LL_PROB_IDX LL_IDX + 4*2*2*33
-#define RE_IDX LL_PROB_IDX + 4
-#define LEAD_IDX RE_IDX + 2*2*2*33
-#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE)
-#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3
-#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN
-#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
-#define OUTPUT_SIZE  POSE_IDX + POSE_SIZE
+#include "clutil.h"
+
+constexpr int MODEL_WIDTH = 512;
+constexpr int MODEL_HEIGHT = 256;
+constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
+
+constexpr int PLAN_MHP_N = 5;
+constexpr int PLAN_MHP_COLUMNS = 30;
+constexpr int PLAN_MHP_VALS = 30*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_MHP_VALS = 4;
+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 MIN_VALID_LEN = 10;
+constexpr int TRAJECTORY_TIME = 10;
+constexpr float TRAJECTORY_DISTANCE = 192.0;
+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 + 4;
+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
-  #define TEMPORAL_SIZE 512
+  constexpr int TEMPORAL_SIZE = 512;
 #else
-  #define TEMPORAL_SIZE 0
+  constexpr int TEMPORAL_SIZE = 0;
 #endif
 
 // #define DUMP_YUV
@@ -35,36 +52,26 @@ Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
 float X_IDXS[TRAJECTORY_SIZE];
 float T_IDXS[TRAJECTORY_SIZE];
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
+void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
-  s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float));
-
-  const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
-  s->output = (float*)calloc(output_size, sizeof(float));
+  s->input_frames = std::make_unique<float[]>(MODEL_FRAME_SIZE * 2);
 
-  s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
+  constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
+  s->output = std::make_unique<float[]>(output_size);
+  s->m = std::make_unique<DefaultRunModel>("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME);
 
 #ifdef TEMPORAL
-  assert(temporal);
   s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
 #endif
 
 #ifdef DESIRE
-  s->prev_desire = std::make_unique<float[]>(DESIRE_LEN);
-  s->pulse_desire = std::make_unique<float[]>(DESIRE_LEN);
-  s->m->addDesire(s->pulse_desire.get(), DESIRE_LEN);
+  s->m->addDesire(s->pulse_desire, DESIRE_LEN);
 #endif
 
 #ifdef TRAFFIC_CONVENTION
-  s->traffic_convention = std::make_unique<float[]>(TRAFFIC_CONVENTION_LEN);
-  s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN);
-
-  bool is_rhd = Params().read_db_bool("IsRHD");
-  if (is_rhd) {
-    s->traffic_convention[1] = 1.0;
-  } else {
-    s->traffic_convention[0] = 1.0;
-  }
+  const int idx = Params().read_db_bool("IsRHD") ? 1 : 0;
+  s->traffic_convention[idx] = 1.0;
+  s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
 #endif
 
   // Build Vandermonde matrix
@@ -75,12 +82,12 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
       vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
     }
   }
+
+  s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
 }
 
-ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
-                           cl_mem yuv_cl, int width, int height,
-                           mat3 transform, void* sock,
-                           float *desire_in) {
+ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
+                           const mat3 &transform, float *desire_in) {
 #ifdef DESIRE
   if (desire_in != NULL) {
     for (int i = 1; i < DESIRE_LEN; i++) {
@@ -98,10 +105,10 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
 
   //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
 
-  float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
+  float *new_frame_buf = frame_prepare(&s->frame, s->q, yuv_cl, width, height, transform);
   memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
   memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
-  s->m->execute(s->input_frames, MODEL_FRAME_SIZE*2);
+  s->m->execute(&s->input_frames[0], MODEL_FRAME_SIZE*2);
 
   #ifdef DUMP_YUV
     FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
@@ -110,7 +117,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
     assert(1==2);
   #endif
 
-  clEnqueueUnmapMemObject(q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL);
+  clEnqueueUnmapMemObject(s->q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL);
 
   // net outputs
   ModelDataRaw net_outputs;
@@ -126,10 +133,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
 }
 
 void model_free(ModelState* s) {
-  free(s->output);
-  free(s->input_frames);
   frame_free(&s->frame);
-  delete s->m;
+  CL_CHECK(clReleaseCommandQueue(s->q));
 }
 
 void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
@@ -157,48 +162,53 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
   out[3] = y0;
 }
 
-void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) {
-  float points_arr[TRAJECTORY_SIZE];
-  float stds_arr[TRAJECTORY_SIZE];
-  float poly_arr[POLYFIT_DEGREE];
-  float std;
-
-  for (int i=0; i<TRAJECTORY_SIZE; i++) {
-    // negative sign because mpc has left positive
-    points_arr[i] = -data[30*i + 16];
-    stds_arr[i] = exp(data[30*(33 + i) + 16]);
+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;
+    }
   }
-  std = stds_arr[0];
-  poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
+  return &data[max_idx * group_size];
+}
 
-  path.setPoly(poly_arr);
-  path.setProb(1.0);
-  path.setStd(std);
-  path.setValidLen(valid_len);
+static const float *get_plan_data(float *plan) {
+  return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, -1);
 }
 
-void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) {
-  float points_arr[TRAJECTORY_SIZE];
-  float stds_arr[TRAJECTORY_SIZE];
-  float poly_arr[POLYFIT_DEGREE];
-  float std;
+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_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob,
+               float valid_len, int valid_len_idx, int ll_idx) {
+  float points[TRAJECTORY_SIZE] = {};
+  float stds[TRAJECTORY_SIZE] = {};
+  float poly[POLYFIT_DEGREE] = {};
 
   for (int i=0; i<TRAJECTORY_SIZE; i++) {
     // negative sign because mpc has left positive
-    points_arr[i] = -data[2*33*ll_idx + 2*i];
-    stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]);
+    if (ll_idx == 0) {
+      points[i] = -data[30 * i + 16];
+      stds[i] = exp(data[30 * (33 + i) + 16]);
+    } else {
+      points[i] = -data[2 * 33 * ll_idx + 2 * i];
+      stds[i] = exp(data[2 * 33 * (4 + ll_idx) + 2 * i]);
+    }
   }
-  std = stds_arr[0];
-  poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx);
+  const float std = stds[0];
+  poly_fit(points, stds, poly, valid_len_idx);
 
-  path.setPoly(poly_arr);
+  path.setPoly(poly);
   path.setProb(prob);
   path.setStd(std);
   path.setValidLen(valid_len);
 }
 
-void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) {
-  lead.setProb(prob);
+void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
+  const float *data = get_lead_data(lead_data, t_offset);
+  lead.setProb(sigmoid(prob[t_offset]));
   lead.setT(t);
   float xyva_arr[LEAD_MHP_VALS];
   float xyva_stds_arr[LEAD_MHP_VALS];
@@ -210,8 +220,9 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d
   lead.setXyvaStd(xyva_stds_arr);
 }
 
-void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) {
-  lead.setProb(prob);
+void fill_lead(cereal::ModelData::LeadData::Builder lead, const float *lead_data, const float *prob, int t_offset) {
+  const float *data = get_lead_data(lead_data, t_offset);
+  lead.setProb(sigmoid(prob[t_offset]));
   lead.setDist(data[0]);
   lead.setStd(exp(data[LEAD_MHP_VALS]));
   // TODO make all msgs same format
@@ -223,7 +234,8 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, fl
   lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
 }
 
-void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
+template <class MetaBuilder>
+void fill_meta(MetaBuilder meta, const float *meta_data) {
   float desire_state_softmax[DESIRE_LEN];
   float desire_pred_softmax[4*DESIRE_LEN];
   softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
@@ -239,22 +251,6 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
   meta.setDesirePrediction(desire_pred_softmax);
 }
 
-void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
-  float desire_state_softmax[DESIRE_LEN] = {};
-  float desire_pred_softmax[4*DESIRE_LEN] = {};
-  softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
-  for (int i=0; i<4; i++) {
-    softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
-            &desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
-  }
-  meta.setDesireState(desire_state_softmax);
-  meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
-  meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
-  meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
-  meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
-  meta.setDesirePrediction(desire_pred_softmax);
-}
-
 void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
                int columns, int column_offset, float * plan_t_arr) {
   float x_arr[TRAJECTORY_SIZE] = {};
@@ -292,34 +288,9 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
   xyzt.setT(t_arr);
 }
 
-
-void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                     uint32_t vipc_dropped_frames, float frame_drop,
-                     const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
-                     float model_execution_time) {
-  // make msg
-  MessageBuilder msg;
-  auto framed = msg.initEvent().initModelV2();
-  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
-  framed.setFrameId(vipc_frame_id);
-  framed.setFrameAge(frame_age);
-  framed.setFrameDropPerc(frame_drop * 100);
-  framed.setTimestampEof(timestamp_eof);
-  framed.setModelExecutionTime(model_execution_time);
-  if (send_raw_pred) {
-    framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
-  }
-
+void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
   // plan
-  int plan_mhp_max_idx = 0;
-  for (int i=1; i<PLAN_MHP_N; i++) {
-    if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
-        net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
-      plan_mhp_max_idx = i;
-    }
-  }
-
-  float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
+  const float *best_plan = get_plan_data(net_outputs.plan);
   float plan_t_arr[TRAJECTORY_SIZE];
   for (int i=0; i<TRAJECTORY_SIZE; i++) {
     plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
@@ -352,59 +323,24 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
   framed.setRoadEdgeStds(road_edge_stds_arr);
 
   // meta
-  auto meta = framed.initMeta();
-  fill_meta_v2(meta, net_outputs.meta);
+  fill_meta(framed.initMeta(), net_outputs.meta);
 
   // leads
   auto leads = framed.initLeads(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++) {
-    int mdn_max_idx = 0;
-    for (int i=1; i<LEAD_MHP_N; i++) {
-      if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
-          net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) {
-        mdn_max_idx = i;
-      }
-    }
-    fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx * (LEAD_MHP_GROUP_SIZE)],
-                 sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]);
+    fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
   }
-  pm.send("modelV2", msg);
 }
 
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop,
-                   const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
-                   float model_execution_time) {
-
-  uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
-  MessageBuilder msg;
-  auto framed = msg.initEvent().initModel();
-  framed.setFrameId(vipc_frame_id);
-  framed.setFrameAge(frame_age);
-  framed.setFrameDropPerc(frame_drop * 100);
-  framed.setTimestampEof(timestamp_eof);
-  framed.setModelExecutionTime(model_execution_time);
-  if (send_raw_pred) {
-    framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
-  }
-
+void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) {
   // Find the distribution that corresponds to the most probable plan
-  int plan_mhp_max_idx = 0;
-  for (int i=1; i<PLAN_MHP_N; i++) {
-    if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
-        net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
-      plan_mhp_max_idx = i;
-    }
-  }
-
+  const float *best_plan = get_plan_data(net_outputs.plan);
   // x pos at 10s is a good valid_len
   float valid_len = 0;
-  float valid_len_candidate;
   for (int i=1; i<TRAJECTORY_SIZE; i++) {
-    valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i];
-    if (valid_len_candidate >= valid_len){
-      valid_len = valid_len_candidate;
+    if (const float len = best_plan[30*i]; len >= valid_len){
+      valid_len = len;
     }
   }
   // clamp to 10 and MODEL_PATH_DISTANCE
@@ -415,47 +351,39 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
       valid_len_idx = i;
     }
   }
+  fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0);
+  fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1);
+  fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2);
 
-  auto lpath = framed.initPath();
-  fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx);
-
-  auto left_lane = framed.initLeftLane();
-  int ll_idx = 1;
-  fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
-            sigmoid(net_outputs.lane_lines_prob[ll_idx]));
-  auto right_lane = framed.initRightLane();
-  ll_idx = 2;
-  fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
-            sigmoid(net_outputs.lane_lines_prob[ll_idx]));
-
-  // Find the distribution that corresponds to the current lead
-  int mdn_max_idx = 0;
-  int t_offset = 0;
-  for (int i=1; i<LEAD_MHP_N; i++) {
-    if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
-        net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
-      mdn_max_idx = i;
-    }
-  }
-  fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
-  // Find the distribution that corresponds to the lead in 2s
-  mdn_max_idx = 0;
-  t_offset = 1;
-  for (int i=1; i<LEAD_MHP_N; i++) {
-    if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
-        net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
-      mdn_max_idx = i;
-    }
-  }
-  fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
+  fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0);
+  fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1);
 
   fill_meta(framed.initMeta(), net_outputs.meta);
+}
 
-  pm.send("model", msg);
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+                   const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
+                   float model_execution_time) {
+  const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
+  auto do_publish = [&](auto init_model_func, const char *pub_name) {
+    MessageBuilder msg;
+    auto framed = (msg.initEvent().*(init_model_func))();
+    framed.setFrameId(vipc_frame_id);
+    framed.setFrameAge(frame_age);
+    framed.setFrameDropPerc(frame_drop * 100);
+    framed.setTimestampEof(timestamp_eof);
+    framed.setModelExecutionTime(model_execution_time);
+    if (send_raw_pred) {
+      framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float)));
+    }
+    fill_model(framed, net_outputs);
+    pm.send(pub_name, msg);
+  };
+  do_publish(&cereal::Event::Builder::initModel, "model");
+  do_publish(&cereal::Event::Builder::initModelV2, "modelV2");
 }
 
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                     uint32_t vipc_dropped_frames, float frame_drop,
+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];
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index 1f12e5e377..c5891013b4 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -1,12 +1,10 @@
-#ifndef MODEL_H
-#define MODEL_H
+#pragma once
 
 // gate this here
 #define TEMPORAL
 #define DESIRE
 #define TRAFFIC_CONVENTION
 
-
 #include "common/mat.h"
 #include "common/util.h"
 #include "common/modeldata.h"
@@ -17,29 +15,9 @@
 #include <memory>
 #include "messaging.hpp"
 
-#define MODEL_NAME "supercombo_dlc"
-
-#define MODEL_WIDTH 512
-#define MODEL_HEIGHT 256
-#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
-#define DESIRE_LEN 8
-#define TRAFFIC_CONVENTION_LEN 2
-
-#define PLAN_MHP_N 5
-#define PLAN_MHP_COLUMNS 30
-#define PLAN_MHP_VALS 30*33
-#define PLAN_MHP_SELECTION 1
-#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION)
-
-#define LEAD_MHP_N 5
-#define LEAD_MHP_VALS 4
-#define LEAD_MHP_SELECTION 3
-#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION)
-
-#define POSE_SIZE 12
-
-#define MODEL_FREQ 20
-
+constexpr int DESIRE_LEN = 8;
+constexpr int TRAFFIC_CONVENTION_LEN = 2;
+constexpr int MODEL_FREQ = 20;
 struct ModelDataRaw {
     float *plan;
     float *lane_lines;
@@ -56,33 +34,26 @@ struct ModelDataRaw {
 
 typedef struct ModelState {
   ModelFrame frame;
-  float *output;
-  float *input_frames;
-  RunModel *m;
+  std::unique_ptr<float[]> output;
+  std::unique_ptr<float[]> input_frames;
+  std::unique_ptr<RunModel> m;
+  cl_command_queue q;
 #ifdef DESIRE
-  std::unique_ptr<float[]> prev_desire;
-  std::unique_ptr<float[]> pulse_desire;
+  float prev_desire[DESIRE_LEN] = {};
+  float pulse_desire[DESIRE_LEN] = {};
 #endif
 #ifdef TRAFFIC_CONVENTION
-  std::unique_ptr<float[]> traffic_convention;
+  float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
 #endif
 } ModelState;
 
-void model_init(ModelState* s, cl_device_id device_id,
-                cl_context context, int temporal);
-ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
-                           cl_mem yuv_cl, int width, int height,
-                           mat3 transform, void* sock, float *desire_in);
+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,
+                           const mat3 &transform, float *desire_in);
 void model_free(ModelState* s);
 void poly_fit(float *in_pts, float *in_stds, float *out);
-
-void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred,
-                   uint64_t timestamp_eof, float model_execution_time);
-void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                   uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred,
-                   uint64_t timestamp_eof, float model_execution_time);
-void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
-                     uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
-                     uint64_t timestamp_eof);
-#endif
+void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
+                   const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
+                   float model_execution_time);
+void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
+                     const ModelDataRaw &net_outputs, uint64_t timestamp_eof);