diff --git a/src/encode_coding_tree.c b/src/encode_coding_tree.c
index 740760d9..1d251a2e 100644
--- a/src/encode_coding_tree.c
+++ b/src/encode_coding_tree.c
@@ -559,122 +559,76 @@ static void encode_inter_prediction_unit(encoder_state_t * const state,
       }
     }
   } else {
-    uint32_t ref_list_idx;
-
-    // Void TEncSbac::codeInterDir( TComDataCU* pcCU, UInt uiAbsPartIdx )
-    if (state->frame->slicetype == KVZ_SLICE_B)
-    {
+    if (state->frame->slicetype == KVZ_SLICE_B) {
       // Code Inter Dir
       uint8_t inter_dir = cur_cu->inter.mv_dir-1;
-      uint8_t ctx = depth;
-      
 
-      if (cur_cu->part_size == SIZE_2Nx2N || (LCU_WIDTH >> depth) != 8)
-      {
-        cabac->cur_ctx = &(cabac->ctx.inter_dir[ctx]);
+      if (cur_cu->part_size == SIZE_2Nx2N || (LCU_WIDTH >> depth) != 8) {
+        cabac->cur_ctx = &(cabac->ctx.inter_dir[depth]);
         CABAC_BIN(cabac, (inter_dir == 2), "inter_pred_idc");
       }
-      if (inter_dir < 2)
-      {
+      if (inter_dir < 2) {
         cabac->cur_ctx = &(cabac->ctx.inter_dir[4]);
         CABAC_BIN(cabac, inter_dir, "inter_pred_idc");
       }
     }
 
-    for (ref_list_idx = 0; ref_list_idx < 2; ref_list_idx++) {
-      if (cur_cu->inter.mv_dir & (1 << ref_list_idx)) {
-
-        // size of the current reference index list (L0/L1)
-        uint8_t ref_LX_size = state->frame->ref_LX_size[ref_list_idx];
-
-        if (ref_LX_size > 1) {
-          // parseRefFrmIdx
-          int32_t ref_frame = cur_cu->inter.mv_ref[ref_list_idx];
-
-          cabac->cur_ctx = &(cabac->ctx.cu_ref_pic_model[0]);
-          CABAC_BIN(cabac, (ref_frame != 0), "ref_idx_lX");
-
-          if (ref_frame > 0) {
-            int32_t i;
-            int32_t ref_num = ref_LX_size - 2;
-
-            cabac->cur_ctx = &(cabac->ctx.cu_ref_pic_model[1]);
-            ref_frame--;
-
-            for (i = 0; i < ref_num; ++i) {
-              const uint32_t symbol = (i == ref_frame) ? 0 : 1;
-
-              if (i == 0) {
-                CABAC_BIN(cabac, symbol, "ref_idx_lX");
-              } else {
-                CABAC_BIN_EP(cabac, symbol, "ref_idx_lX");
-              }
-              if (symbol == 0) break;
-            }
-          }
-        }
-
-        if (!(/*pcCU->getSlice()->getMvdL1ZeroFlag() &&*/ state->frame->ref_list == REF_PIC_LIST_1 && cur_cu->inter.mv_dir == 3)) {
-
-          int16_t mv_cand[2][2];
-          kvz_inter_get_mv_cand_cua(
-              state,
-              x, y, width, height,
-              mv_cand, cur_cu, ref_list_idx);
-
-          uint8_t cu_mv_cand = CU_GET_MV_CAND(cur_cu, ref_list_idx);
-
-          const int32_t mvd_hor = cur_cu->inter.mv[ref_list_idx][0] - mv_cand[cu_mv_cand][0];
-          const int32_t mvd_ver = cur_cu->inter.mv[ref_list_idx][1] - mv_cand[cu_mv_cand][1];
-          const int8_t hor_abs_gr0 = mvd_hor != 0;
-          const int8_t ver_abs_gr0 = mvd_ver != 0;
-          const uint32_t mvd_hor_abs = abs(mvd_hor);
-          const uint32_t mvd_ver_abs = abs(mvd_ver);
-
-
-          cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[0]);
-          CABAC_BIN(cabac, (mvd_hor != 0), "abs_mvd_greater0_flag_hor");
-          CABAC_BIN(cabac, (mvd_ver != 0), "abs_mvd_greater0_flag_ver");
-
-          cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[1]);
-
-          if (hor_abs_gr0) {
-            CABAC_BIN(cabac, (mvd_hor_abs>1), "abs_mvd_greater1_flag_hor");
-          }
-
-          if (ver_abs_gr0) {
-            CABAC_BIN(cabac, (mvd_ver_abs>1), "abs_mvd_greater1_flag_ver");
-          }
-
-          if (hor_abs_gr0) {
-            if (mvd_hor_abs > 1) {
-              kvz_cabac_write_ep_ex_golomb(state, cabac, mvd_hor_abs-2, 1);
-            }
-            uint32_t mvd_hor_sign = (mvd_hor>0)?0:1;
-            if(!state->cabac.only_count)
-              if (state->encoder_control->cfg.crypto_features & KVZ_CRYPTO_MV_SIGNS)
-                mvd_hor_sign = mvd_hor_sign^kvz_crypto_get_key(state->crypto_hdl, 1);
-            CABAC_BIN_EP(cabac, mvd_hor_sign, "mvd_sign_flag_hor");
-          }
-          if (ver_abs_gr0) {
-            if (mvd_ver_abs > 1) {
-              kvz_cabac_write_ep_ex_golomb(state, cabac, mvd_ver_abs-2, 1);
-            }
-            uint32_t mvd_ver_sign = (mvd_ver>0)?0:1;
-            if(!state->cabac.only_count)
-              if (state->encoder_control->cfg.crypto_features & KVZ_CRYPTO_MV_SIGNS)
-                mvd_ver_sign = mvd_ver_sign^kvz_crypto_get_key(state->crypto_hdl, 1);
-            CABAC_BIN_EP(cabac, mvd_ver_sign, "mvd_sign_flag_ver");
-          }
-        }
-
-        // Signal which candidate MV to use
-        kvz_cabac_write_unary_max_symbol(cabac,
-                                         cabac->ctx.mvp_idx_model,
-                                         CU_GET_MV_CAND(cur_cu, ref_list_idx),
-                                         1,
-                                         AMVP_MAX_NUM_CANDS - 1);
+    for (uint32_t ref_list_idx = 0; ref_list_idx < 2; ref_list_idx++) {
+      if (!(cur_cu->inter.mv_dir & (1 << ref_list_idx))) {
+        continue;
       }
+
+      // size of the current reference index list (L0/L1)
+      uint8_t ref_LX_size = state->frame->ref_LX_size[ref_list_idx];
+
+      if (ref_LX_size > 1) {
+        // parseRefFrmIdx
+        int32_t ref_frame = cur_cu->inter.mv_ref[ref_list_idx];
+
+        cabac->cur_ctx = &(cabac->ctx.cu_ref_pic_model[0]);
+        CABAC_BIN(cabac, (ref_frame != 0), "ref_idx_lX");
+
+        if (ref_frame > 0) {
+          ref_frame--;
+
+          int32_t ref_num = ref_LX_size - 2;
+
+          for (int32_t i = 0; i < ref_num; ++i) {
+            const uint32_t symbol = (i == ref_frame) ? 0 : 1;
+
+            if (i == 0) {
+              cabac->cur_ctx = &cabac->ctx.cu_ref_pic_model[1];
+              CABAC_BIN(cabac, symbol, "ref_idx_lX");
+            } else {
+              CABAC_BIN_EP(cabac, symbol, "ref_idx_lX");
+            }
+            if (symbol == 0) break;
+          }
+        }
+      }
+
+      if (state->frame->ref_list != REF_PIC_LIST_1 || cur_cu->inter.mv_dir != 3) {
+
+        int16_t mv_cand[2][2];
+        kvz_inter_get_mv_cand_cua(
+            state,
+            x, y, width, height,
+            mv_cand, cur_cu, ref_list_idx);
+
+        uint8_t cu_mv_cand = CU_GET_MV_CAND(cur_cu, ref_list_idx);
+        const int32_t mvd_hor = cur_cu->inter.mv[ref_list_idx][0] - mv_cand[cu_mv_cand][0];
+        const int32_t mvd_ver = cur_cu->inter.mv[ref_list_idx][1] - mv_cand[cu_mv_cand][1];
+
+        kvz_encode_mvd(state, cabac, mvd_hor, mvd_ver);
+      }
+
+      // Signal which candidate MV to use
+      kvz_cabac_write_unary_max_symbol(cabac,
+                                       cabac->ctx.mvp_idx_model,
+                                       CU_GET_MV_CAND(cur_cu, ref_list_idx),
+                                       1,
+                                       AMVP_MAX_NUM_CANDS - 1);
+
     } // for ref_list
   } // if !merge
 }
@@ -1186,3 +1140,52 @@ void kvz_encode_coding_tree(encoder_state_t * const state,
     exit(1);
   }
 }
+
+
+void kvz_encode_mvd(encoder_state_t * const state,
+                    cabac_data_t *cabac,
+                    int32_t mvd_hor,
+                    int32_t mvd_ver)
+{
+  const int8_t hor_abs_gr0 = mvd_hor != 0;
+  const int8_t ver_abs_gr0 = mvd_ver != 0;
+  const uint32_t mvd_hor_abs = abs(mvd_hor);
+  const uint32_t mvd_ver_abs = abs(mvd_ver);
+
+  cabac->cur_ctx = &cabac->ctx.cu_mvd_model[0];
+  CABAC_BIN(cabac, (mvd_hor != 0), "abs_mvd_greater0_flag_hor");
+  CABAC_BIN(cabac, (mvd_ver != 0), "abs_mvd_greater0_flag_ver");
+
+  cabac->cur_ctx = &cabac->ctx.cu_mvd_model[1];
+  if (hor_abs_gr0) {
+    CABAC_BIN(cabac, (mvd_hor_abs>1), "abs_mvd_greater1_flag_hor");
+  }
+  if (ver_abs_gr0) {
+    CABAC_BIN(cabac, (mvd_ver_abs>1), "abs_mvd_greater1_flag_ver");
+  }
+
+  if (hor_abs_gr0) {
+    if (mvd_hor_abs > 1) {
+      kvz_cabac_write_ep_ex_golomb(state, cabac, mvd_hor_abs - 2, 1);
+    }
+    uint32_t mvd_hor_sign = (mvd_hor > 0) ? 0 : 1;
+    if (!state->cabac.only_count &&
+        state->encoder_control->cfg.crypto_features & KVZ_CRYPTO_MV_SIGNS)
+    {
+      mvd_hor_sign = mvd_hor_sign ^ kvz_crypto_get_key(state->crypto_hdl, 1);
+    }
+    CABAC_BIN_EP(cabac, mvd_hor_sign, "mvd_sign_flag_hor");
+  }
+  if (ver_abs_gr0) {
+    if (mvd_ver_abs > 1) {
+      kvz_cabac_write_ep_ex_golomb(state, cabac, mvd_ver_abs - 2, 1);
+    }
+    uint32_t mvd_ver_sign = mvd_ver > 0 ? 0 : 1;
+    if (!state->cabac.only_count &&
+        state->encoder_control->cfg.crypto_features & KVZ_CRYPTO_MV_SIGNS)
+    {
+      mvd_ver_sign = mvd_ver_sign^kvz_crypto_get_key(state->crypto_hdl, 1);
+    }
+    CABAC_BIN_EP(cabac, mvd_ver_sign, "mvd_sign_flag_ver");
+  }
+}
diff --git a/src/encode_coding_tree.h b/src/encode_coding_tree.h
index 68e6e2ba..e0b258e8 100644
--- a/src/encode_coding_tree.h
+++ b/src/encode_coding_tree.h
@@ -42,4 +42,9 @@ void kvz_encode_coeff_nxn(encoder_state_t * const state,
                           int8_t scan_mode,
                           int8_t tr_skip);
 
+void kvz_encode_mvd(encoder_state_t * const state,
+                    cabac_data_t *cabac,
+                    int32_t mvd_hor,
+                    int32_t mvd_ver);
+
 #endif // ENCODE_CODING_TREE_H_
diff --git a/src/rdo.c b/src/rdo.c
index 31e1fdd8..813aeede 100644
--- a/src/rdo.c
+++ b/src/rdo.c
@@ -852,52 +852,23 @@ void kvz_rdoq(encoder_state_t * const state, coeff_t *coef, coeff_t *dest_coeff,
   }
 }
 
-/** MVD cost calculation with CABAC
-* \returns int
-* Calculates cost of actual motion vectors using CABAC coding
-*/
+/**
+ * Calculate cost of actual motion vectors using CABAC coding
+ */
 uint32_t kvz_get_mvd_coding_cost_cabac(const encoder_state_t *state,
-                                       vector2d_t *mvd,
-                                       const cabac_data_t* real_cabac)
+                                       const cabac_data_t* cabac,
+                                       const int32_t mvd_hor,
+                                       const int32_t mvd_ver)
 {
-  uint32_t bitcost = 0;
-  const int32_t mvd_hor = mvd->x;
-  const int32_t mvd_ver = mvd->y;
-  const int8_t hor_abs_gr0 = mvd_hor != 0;
-  const int8_t ver_abs_gr0 = mvd_ver != 0;
-  const uint32_t mvd_hor_abs = abs(mvd_hor);
-  const uint32_t mvd_ver_abs = abs(mvd_ver);
+  cabac_data_t cabac_copy = *cabac;
+  cabac_copy.only_count = 1;
 
-  cabac_data_t cabac_copy;
-  memcpy(&cabac_copy, real_cabac, sizeof(cabac_data_t));
-  cabac_data_t *cabac = &cabac_copy;
-  cabac->only_count = 1;
+  // It is safe to drop const here because cabac->only_count is set.
+  kvz_encode_mvd((encoder_state_t*) state, &cabac_copy, mvd_hor, mvd_ver);
 
-  cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[0]);
-  CABAC_BIN(cabac, (mvd_hor != 0), "abs_mvd_greater0_flag_hor");
-  CABAC_BIN(cabac, (mvd_ver != 0), "abs_mvd_greater0_flag_ver");
-  cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[1]);
-  if (hor_abs_gr0) {
-    CABAC_BIN(cabac, (mvd_hor_abs > 1), "abs_mvd_greater1_flag_hor");
-  }
-  if (ver_abs_gr0) {
-    CABAC_BIN(cabac, (mvd_ver_abs > 1), "abs_mvd_greater1_flag_ver");
-  }
-  if (hor_abs_gr0) {
-    if (mvd_hor_abs > 1) {
-      // It is safe to drop const here because cabac->only_count is set.
-      kvz_cabac_write_ep_ex_golomb((encoder_state_t*)state, cabac, mvd_hor_abs - 2, 1);
-    }
-    CABAC_BIN_EP(cabac, (mvd_hor > 0) ? 0 : 1, "mvd_sign_flag_hor");
-  }
-  if (ver_abs_gr0) {
-    if (mvd_ver_abs > 1) {
-      // It is safe to drop const here because cabac->only_count is set.
-      kvz_cabac_write_ep_ex_golomb((encoder_state_t*)state, cabac, mvd_ver_abs - 2, 1);
-    }
-    CABAC_BIN_EP(cabac, (mvd_ver > 0) ? 0 : 1, "mvd_sign_flag_ver");
-  }
-  bitcost = ((23 - cabac->bits_left) + (cabac->num_buffered_bytes << 3)) - ((23 - real_cabac->bits_left) + (real_cabac->num_buffered_bytes << 3));
+  uint32_t bitcost =
+    ((23 - cabac_copy.bits_left) + (cabac_copy.num_buffered_bytes << 3)) -
+    ((23 - cabac->bits_left)     + (cabac->num_buffered_bytes << 3));
 
   return bitcost;
 }
@@ -919,8 +890,7 @@ uint32_t kvz_calc_mvd_cost_cabac(const encoder_state_t * state,
   cabac_data_t state_cabac_copy;
   cabac_data_t* cabac;
   uint32_t merge_idx;
-  int cand1_cost, cand2_cost;
-  vector2d_t mvd_temp1, mvd_temp2, mvd = { 0, 0 };
+  vector2d_t mvd = { 0, 0 };
   int8_t merged = 0;
   int8_t cur_mv_cand = 0;
 
@@ -952,20 +922,23 @@ uint32_t kvz_calc_mvd_cost_cabac(const encoder_state_t * state,
   cabac = &state_cabac_copy;
 
   if (!merged) {
-    mvd_temp1.x = x - mv_cand[0][0];
-    mvd_temp1.y = y - mv_cand[0][1];
-    cand1_cost = kvz_get_mvd_coding_cost_cabac(state, &mvd_temp1, cabac);
-
-    mvd_temp2.x = x - mv_cand[1][0];
-    mvd_temp2.y = y - mv_cand[1][1];
-    cand2_cost = kvz_get_mvd_coding_cost_cabac(state, &mvd_temp2, cabac);
+    vector2d_t mvd1 = {
+      x - mv_cand[0][0],
+      y - mv_cand[0][1],
+    };
+    vector2d_t mvd2 = {
+      x - mv_cand[1][0],
+      y - mv_cand[1][1],
+    };
+    uint32_t cand1_cost = kvz_get_mvd_coding_cost_cabac(state, cabac, mvd1.x, mvd1.y);
+    uint32_t cand2_cost = kvz_get_mvd_coding_cost_cabac(state, cabac, mvd2.x, mvd2.y);
 
     // Select candidate 1 if it has lower cost
     if (cand2_cost < cand1_cost) {
       cur_mv_cand = 1;
-      mvd = mvd_temp2;
+      mvd = mvd2;
     } else {
-      mvd = mvd_temp1;
+      mvd = mvd1;
     }
   }
 
@@ -1031,51 +1004,18 @@ uint32_t kvz_calc_mvd_cost_cabac(const encoder_state_t * state,
 
         // ToDo: Bidir vector support
         if (!(state->frame->ref_list == REF_PIC_LIST_1 && /*cur_cu->inter.mv_dir == 3*/ 0)) {
-          const int32_t mvd_hor = mvd.x;
-          const int32_t mvd_ver = mvd.y;
-          const int8_t hor_abs_gr0 = mvd_hor != 0;
-          const int8_t ver_abs_gr0 = mvd_ver != 0;
-          const uint32_t mvd_hor_abs = abs(mvd_hor);
-          const uint32_t mvd_ver_abs = abs(mvd_ver);
-
-          cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[0]);
-          CABAC_BIN(cabac, (mvd_hor != 0), "abs_mvd_greater0_flag_hor");
-          CABAC_BIN(cabac, (mvd_ver != 0), "abs_mvd_greater0_flag_ver");
-
-          cabac->cur_ctx = &(cabac->ctx.cu_mvd_model[1]);
-
-          if (hor_abs_gr0) {
-            CABAC_BIN(cabac, (mvd_hor_abs > 1), "abs_mvd_greater1_flag_hor");
-          }
-
-          if (ver_abs_gr0) {
-            CABAC_BIN(cabac, (mvd_ver_abs > 1), "abs_mvd_greater1_flag_ver");
-          }
-
-          if (hor_abs_gr0) {
-            if (mvd_hor_abs > 1) {
-              // It is safe to drop const because cabac->only_count is set.
-              kvz_cabac_write_ep_ex_golomb((encoder_state_t*)state, cabac, mvd_hor_abs - 2, 1);
-            }
-
-            CABAC_BIN_EP(cabac, (mvd_hor > 0) ? 0 : 1, "mvd_sign_flag_hor");
-          }
-
-          if (ver_abs_gr0) {
-            if (mvd_ver_abs > 1) {
-              // It is safe to drop const because cabac->only_count is set.
-              kvz_cabac_write_ep_ex_golomb((encoder_state_t*)state, cabac, mvd_ver_abs - 2, 1);
-            }
-
-            CABAC_BIN_EP(cabac, (mvd_ver > 0) ? 0 : 1, "mvd_sign_flag_ver");
-          }
+          // It is safe to drop const here because cabac->only_count is set.
+          kvz_encode_mvd((encoder_state_t*) state, cabac, mvd.x, mvd.y);
         }
 
         // Signal which candidate MV to use
-        kvz_cabac_write_unary_max_symbol(cabac, cabac->ctx.mvp_idx_model, cur_mv_cand, 1,
-          AMVP_MAX_NUM_CANDS - 1);
+        kvz_cabac_write_unary_max_symbol(
+            cabac,
+            cabac->ctx.mvp_idx_model,
+            cur_mv_cand,
+            1,
+            AMVP_MAX_NUM_CANDS - 1);
       }
-
     }
   }
 
diff --git a/src/rdo.h b/src/rdo.h
index 8a8e0022..72450fb7 100644
--- a/src/rdo.h
+++ b/src/rdo.h
@@ -58,8 +58,9 @@ uint32_t kvz_get_coded_level(encoder_state_t * state, double* coded_cost, double
 kvz_mvd_cost_func kvz_calc_mvd_cost_cabac;
 
 uint32_t kvz_get_mvd_coding_cost_cabac(const encoder_state_t *state,
-                                       vector2d_t *mvd,
-                                       const cabac_data_t* cabac);
+                                       const cabac_data_t* cabac,
+                                       int32_t mvd_hor,
+                                       int32_t mvd_ver);
 
 // Number of fixed point fractional bits used in the fractional bit table.
 #define CTX_FRAC_BITS 15
diff --git a/src/search_inter.c b/src/search_inter.c
index f0aa11ef..306f89e1 100644
--- a/src/search_inter.c
+++ b/src/search_inter.c
@@ -307,11 +307,12 @@ static void select_starting_point(inter_search_info_t *info, vector2d_t extra_mv
 
 
 static uint32_t get_mvd_coding_cost(const encoder_state_t *state,
-                                    vector2d_t *mvd,
-                                    const cabac_data_t* cabac)
+                                    const cabac_data_t* cabac,
+                                    const int32_t mvd_hor,
+                                    const int32_t mvd_ver)
 {
   unsigned bitcost = 0;
-  const vector2d_t abs_mvd = { abs(mvd->x), abs(mvd->y) };
+  const vector2d_t abs_mvd = { abs(mvd_hor), abs(mvd_ver) };
 
   bitcost += CTX_ENTROPY_BITS(&cabac->ctx.cu_mvd_model[0], abs_mvd.x > 0);
   if (abs_mvd.x > 0) {
@@ -336,6 +337,53 @@ static uint32_t get_mvd_coding_cost(const encoder_state_t *state,
 }
 
 
+static int select_mv_cand(const encoder_state_t *state,
+                          int16_t mv_cand[2][2],
+                          int32_t mv_x,
+                          int32_t mv_y,
+                          uint32_t *cost_out)
+{
+  const bool same_cand =
+    (mv_cand[0][0] == mv_cand[1][0] && mv_cand[0][1] == mv_cand[1][1]);
+
+  if (same_cand && !cost_out) {
+    // Pick the first one if both candidates are the same.
+    return 0;
+  }
+
+  uint32_t (*mvd_coding_cost)(const encoder_state_t * const state,
+                              const cabac_data_t*,
+                              int32_t, int32_t);
+  if (state->encoder_control->cfg.mv_rdo) {
+    mvd_coding_cost = kvz_get_mvd_coding_cost_cabac;
+  } else {
+    mvd_coding_cost = get_mvd_coding_cost;
+  }
+
+  uint32_t cand1_cost = mvd_coding_cost(
+      state, &state->cabac,
+      mv_x - mv_cand[0][0],
+      mv_y - mv_cand[0][1]);
+
+  uint32_t cand2_cost;
+  if (same_cand) {
+    cand2_cost = cand1_cost;
+  } else {
+    cand2_cost = mvd_coding_cost(
+      state, &state->cabac,
+      mv_x - mv_cand[1][0],
+      mv_y - mv_cand[1][1]);
+  }
+
+  if (cost_out) {
+    *cost_out = MIN(cand1_cost, cand2_cost);
+  }
+
+  // Pick the second candidate if it has lower cost.
+  return cand2_cost < cand1_cost ? 1 : 0;
+}
+
+
 static uint32_t calc_mvd_cost(const encoder_state_t *state,
                               int x,
                               int y,
@@ -348,10 +396,7 @@ static uint32_t calc_mvd_cost(const encoder_state_t *state,
 {
   uint32_t temp_bitcost = 0;
   uint32_t merge_idx;
-  int cand1_cost,cand2_cost;
-  vector2d_t mvd_temp1, mvd_temp2;
   int8_t merged      = 0;
-  int8_t cur_mv_cand = 0;
 
   x *= 1 << mv_shift;
   y *= 1 << mv_shift;
@@ -371,20 +416,10 @@ static uint32_t calc_mvd_cost(const encoder_state_t *state,
   }
 
   // Check mvd cost only if mv is not merged
-  if(!merged) {
-    mvd_temp1.x = x - mv_cand[0][0];
-    mvd_temp1.y = y - mv_cand[0][1];
-    cand1_cost = get_mvd_coding_cost(state, &mvd_temp1, &state->cabac);
-
-    mvd_temp2.x = x - mv_cand[1][0];
-    mvd_temp2.y = y - mv_cand[1][1];
-    cand2_cost = get_mvd_coding_cost(state, &mvd_temp2, &state->cabac);
-
-    // Select candidate 1 if it has lower cost
-    if (cand2_cost < cand1_cost) {
-      cur_mv_cand = 1;
-    }
-    temp_bitcost += cur_mv_cand ? cand2_cost : cand1_cost;
+  if (!merged) {
+    uint32_t mvd_cost = 0;
+    select_mv_cand(state, mv_cand, x, y, &mvd_cost);
+    temp_bitcost += mvd_cost;
   }
   *bitcost = temp_bitcost;
   return temp_bitcost*(int32_t)(state->lambda_sqrt + 0.5);
@@ -1315,30 +1350,9 @@ static void search_pu_inter_ref(inter_search_info_t *info,
 
   // Only check when candidates are different
   int cu_mv_cand = 0;
-  if (!merged && (
-        info->mv_cand[0][0] != info->mv_cand[1][0] ||
-        info->mv_cand[0][1] != info->mv_cand[1][1]))
-  {
-    uint32_t (*mvd_coding_cost)(const encoder_state_t * const state,
-                                vector2d_t *,
-                                const cabac_data_t*) =
-      cfg->mv_rdo ? kvz_get_mvd_coding_cost_cabac : get_mvd_coding_cost;
-
-    vector2d_t mvd_temp1, mvd_temp2;
-    int cand1_cost,cand2_cost;
-
-    mvd_temp1.x = mv.x - info->mv_cand[0][0];
-    mvd_temp1.y = mv.y - info->mv_cand[0][1];
-    cand1_cost = mvd_coding_cost(info->state, &mvd_temp1, &info->state->cabac);
-
-    mvd_temp2.x = mv.x - info->mv_cand[1][0];
-    mvd_temp2.y = mv.y - info->mv_cand[1][1];
-    cand2_cost = mvd_coding_cost(info->state, &mvd_temp2, &info->state->cabac);
-
-    // Select candidate 1 if it has lower cost
-    if (cand2_cost < cand1_cost) {
-      cu_mv_cand = 1;
-    }
+  if (!merged) {
+    cu_mv_cand =
+      select_mv_cand(info->state, info->mv_cand, mv.x, mv.y, NULL);
   }
 
   if (info->best_cost < *inter_cost) {
@@ -1465,7 +1479,6 @@ static void search_pu_inter(encoder_state_t * const state,
         {
           uint32_t bitcost[2];
           uint32_t cost = 0;
-          int8_t cu_mv_cand = 0;
           int16_t mv[2][2];
           kvz_pixel tmp_block[64 * 64];
           kvz_pixel tmp_pic[64 * 64];
@@ -1558,32 +1571,13 @@ static void search_pu_inter(encoder_state_t * const state,
 
             // Each motion vector has its own candidate
             for (int reflist = 0; reflist < 2; reflist++) {
-              cu_mv_cand = 0;
               kvz_inter_get_mv_cand(state, x, y, width, height, info.mv_cand, cur_cu, lcu, reflist);
-              if (info.mv_cand[0][0] != info.mv_cand[1][0] ||
-                  info.mv_cand[0][1] != info.mv_cand[1][1])
-              {
-                uint32_t (*mvd_coding_cost)(const encoder_state_t * const state,
-                                            vector2d_t *,
-                                            const cabac_data_t*) =
-                  cfg->mv_rdo ? kvz_get_mvd_coding_cost_cabac : get_mvd_coding_cost;
-
-                vector2d_t mvd_temp1, mvd_temp2;
-                int cand1_cost, cand2_cost;
-
-                mvd_temp1.x = cur_cu->inter.mv[reflist][0] - info.mv_cand[0][0];
-                mvd_temp1.y = cur_cu->inter.mv[reflist][1] - info.mv_cand[0][1];
-                cand1_cost = mvd_coding_cost(state, &mvd_temp1, (cabac_data_t*)&state->cabac);
-
-                mvd_temp2.x = cur_cu->inter.mv[reflist][0] - info.mv_cand[1][0];
-                mvd_temp2.y = cur_cu->inter.mv[reflist][1] - info.mv_cand[1][1];
-                cand2_cost = mvd_coding_cost(state, &mvd_temp2, (cabac_data_t*)&state->cabac);
-
-                // Select candidate 1 if it has lower cost
-                if (cand2_cost < cand1_cost) {
-                  cu_mv_cand = 1;
-                }
-              }
+              int cu_mv_cand = select_mv_cand(
+                  state,
+                  info.mv_cand,
+                  cur_cu->inter.mv[reflist][0],
+                  cur_cu->inter.mv[reflist][1],
+                  NULL);
               CU_SET_MV_CAND(cur_cu, reflist, cu_mv_cand);
             }