diff --git a/src/global.h b/src/global.h
index 463835b2..89ef57f4 100644
--- a/src/global.h
+++ b/src/global.h
@@ -47,6 +47,7 @@
 #define MIN(a,b) (((a)<(b))?(a):(b))
 #define CLIP(low,high,value) MAX((low),MIN((high),(value)))
 #define SWAP(a,b,swaptype) { swaptype tempval; tempval = a; a = b; b = tempval; }
+#define LCU_WIDTH_FROM_DEPTH(depth) (LCU_WIDTH >> depth)
 
 #define VERSION_STRING "0.2               "
 #define VERSION 0.2
diff --git a/src/picture.c b/src/picture.c
index 9a25b653..9e667998 100644
--- a/src/picture.c
+++ b/src/picture.c
@@ -436,23 +436,6 @@ uint32_t Hadamard8x8(int16_t *piOrg, int32_t iStrideOrg, int16_t *piCur, int32_t
   return sad;
 }
 
-
-//Sum of Absolute Difference for block
-uint32_t SAD(uint8_t *block,uint8_t* block2, uint32_t x, uint32_t y)
-{
-  uint32_t i;
-  uint32_t sum=0;
-  for(i=0;i<x*y;i+=4)
-  {
-    sum+=abs((int32_t)block[i]-(int32_t)block2[i]);
-    sum+=abs((int32_t)block[i+1]-(int32_t)block2[i+1]);
-    sum+=abs((int32_t)block[i+2]-(int32_t)block2[i+2]);
-    sum+=abs((int32_t)block[i+3]-(int32_t)block2[i+3]);
-  }
-
-  return sum;
-}
-
 uint32_t SAD64x64(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stride2)
 {
   int32_t y,x;
@@ -632,4 +615,27 @@ uint32_t SAD4x4(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stride
   }
 
   return sum;
-}
\ No newline at end of file
+}
+
+/**
+ * Calculate Sum of Absolute Differences (SAD) between two rectangular regions located in arbitrary points in the picture.
+ * 
+ * data1 is the starting point of the first picture.
+ * data2 is the starting point of the second picture.
+ * width is the width of the region for which SAD is calculated.
+ * height is the height of the region for which SAD is calculated.
+ * stride is the width of the pixel array.
+ */
+uint32_t SAD(uint8_t *data1, uint8_t *data2, unsigned width, unsigned height, unsigned stride)
+{
+  unsigned y, x;
+  unsigned sad = 0;
+
+  for (y = 0; y < height; ++y) {
+    for (x = 0; x < width; ++x) {
+      sad += abs((int)data1[y * stride + x] - (int)data2[y * stride + x]);
+    }
+  }
+
+  return sad;
+}
diff --git a/src/picture.h b/src/picture.h
index 78890d16..4aa4126b 100644
--- a/src/picture.h
+++ b/src/picture.h
@@ -21,6 +21,8 @@ uint32_t SAD32x32(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stri
 uint32_t SAD16x16(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stride2);
 uint32_t SAD8x8(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stride2);
 uint32_t SAD4x4(int16_t *block,uint32_t stride1,int16_t* block2, uint32_t stride2);
+uint32_t SAD(uint8_t *data1, uint8_t *data2, unsigned width, unsigned height, unsigned stride);
+
 double imagePSNR(uint8_t *frame1, uint8_t *frame2, int32_t x, int32_t y);
 
 /** \defgroup picture_group Picture handler group
diff --git a/src/search.c b/src/search.c
index 695acb77..1c070512 100644
--- a/src/search.c
+++ b/src/search.c
@@ -25,6 +25,38 @@
 #include "filter.h"
 #include "search.h"
 
+
+/**
+ * 
+ *
+ * pic: 
+ * pic_data: picture color data starting from the block MV is being searched for.
+ * ref_data: picture color data starting from the beginning of reference pic.
+ * cur_cu: 
+ */
+void search_motion_vector(picture *pic, uint8_t *pic_data, uint8_t *ref_data, CU_info *cur_cu,  unsigned step, int x, int y)
+{
+  // TODO: Inter: Handle non-square blocks.
+  unsigned block_width = LCU_WIDTH_FROM_DEPTH(cur_cu->depth);
+  unsigned block_height = block_width;
+
+  // TODO: Inter: Calculating error outside picture borders.
+  // This prevents choosing vectors that need interpolating of borders to work.
+  if (x < 0 || y < 0 || x < pic->width - LCU_WIDTH || pic->height - LCU_WIDTH) return;
+
+  cur_cu->inter.mv[0] = x;
+  cur_cu->inter.mv[1] = y;
+  cur_cu->inter.cost = SAD(pic_data, &ref_data[y * pic->width + x], block_width, block_height, pic->width);
+
+  step /= 2;
+  if (step > 0) {
+    search_motion_vector(pic, pic_data, ref_data, cur_cu, step, x, y - step);
+    search_motion_vector(pic, pic_data, ref_data, cur_cu, step, x - step, y);
+    search_motion_vector(pic, pic_data, ref_data, cur_cu, step, x + step, y);
+    search_motion_vector(pic, pic_data, ref_data, cur_cu, step, x, y + step);
+  }
+}
+
 void search_buildReferenceBorder(picture* pic, int32_t xCtb, int32_t yCtb,int16_t outwidth, int16_t* dst, int32_t dststride, int8_t chroma)
 {
   int32_t leftColumn;  /*!< left column iterator */
@@ -165,14 +197,17 @@ void search_tree(encoder_control* encoder,uint16_t xCtb,uint16_t yCtb, uint8_t d
 
       }
 
-      cur_CU->type = CU_INTER;
-      cur_CU->inter.mv[0] = 0<<2;
-      cur_CU->inter.mv[1] = 0<<2;
-      if(xCtb == 0 && yCtb == 0)
       {
-        cur_CU->inter.mv[1] = 0<<2;
+        unsigned mv[2] = { 0, 0 }; // TODO: Take initial MV from adjacent blocks.
+        picture *cur_pic = encoder->in.cur_pic;
+        uint8_t *cur_data = &cur_pic->yData[(mv[1] * cur_pic->width) + mv[0]];
+        
+        picture *ref_pic = encoder->ref->pics[0];
+
+        search_motion_vector(cur_pic, cur_data, ref_pic->yData, cur_CU, cur_pic->width >> 1, mv[0], mv[1]);
       }
-      cur_CU->inter.cost = 10;
+
+      cur_CU->type = CU_INTER;
       cur_CU->inter.mv_dir = 1;
       inter_setBlockMode(encoder->in.cur_pic,xCtb,yCtb,depth,cur_CU);
     }
@@ -291,4 +326,4 @@ void search_slice_data(encoder_control* encoder)
       search_best_mode(encoder, xCtb<<MAX_DEPTH,yCtb<<MAX_DEPTH, depth);
     }
   }
-}
\ No newline at end of file
+}