diff --git a/raycastlib.h b/raycastlib.h
index 6d70ffd..dd178ad 100644
--- a/raycastlib.h
+++ b/raycastlib.h
@@ -19,6 +19,25 @@
 typedef int32_t Unit;   /**< Smallest spatial unit, there is UNITS_PER_SQUARE
                              units in a square's length. */
 
+#define logVector2D(v)\
+  printf("[%d,%d]\n",v.x,v.y);
+
+#define logRay(r)\
+  printf("ray:\n");\
+  printf("  start: ");\
+  logVector2D(r.start);\
+  printf("  dir: ");\
+  logVector2D(r.direction);\
+
+#define logHitResult(h)\
+  printf("hit:\n");\
+  printf("  sqaure: ");\
+  logVector2D(h.square);\
+  printf("  pos: ");\
+  logVector2D(h.position);\
+  printf("  dist: %d", h.distance);\
+  printf("  texcoord: %d", h.textureCoord);\
+
 /// Position in 2D space.
 typedef struct
 {
@@ -68,6 +87,17 @@ void castRayMultiHit(Ray ray, int16_t (*arrayFunc)(int16_t, int16_t),
 //=============================================================================
 // privates
 
+Unit clamp(Unit value, Unit valueMin, Unit valueMax)
+{
+  if (value < valueMin)
+    return valueMin;
+
+  if (value > valueMax)
+    return valueMax;
+
+  return value;
+}
+
 uint16_t sqrtInt(uint32_t value)
 {
   uint32_t result = 0;
@@ -123,9 +153,10 @@ void castRaySquare(Ray localRay, Vector2D *nextCellOffset,
     {\
       nextCellOffset->c1 = n;\
       collisionPointOffset->c1 = criticalLine.start.c1 - localRay.start.c1;\
-      collisionPointOffset->c2 =\
+      collisionPointOffset->c2 = clamp(\
         (collisionPointOffset->c1 * localRay.direction.c2) /\
-        (localRay.direction.c1 == 0 ? 1 : localRay.direction.c1);\
+        (localRay.direction.c1 == 0 ? 1 : localRay.direction.c1),\
+        0,UNITS_PER_SQUARE - 1);\
     }
 
   #define helper2(n1,n2,c)\
diff --git a/test.c b/test.c
index 323459e..dfcecc2 100644
--- a/test.c
+++ b/test.c
@@ -1,33 +1,11 @@
-#include "raycastlib.h"
 #include <stdio.h>
-
-void logVector2D(Vector2D v)
-{
-  printf("[%d,%d]\n",v.x,v.y);
-}
-
-void logRay(Ray r)
-{
-  printf("ray:\n");
-  printf("  start: ");
-  logVector2D(r.start);
-  printf("  dir: ");
-  logVector2D(r.direction);
-}
-
-void logHitResult(HitResult h)
-{
-  printf("hit:\n");\
-  printf("  sqaure: ");
-  logVector2D(h.square);
-  printf("  pos: ");
-  logVector2D(h.position);
-  printf("  dist: %d", h.distance);
-  printf("  texcoord: %d", h.textureCoord);
-}
+#include "raycastlib.h"
 
 int16_t testArrayFunc(int16_t x, int16_t y)
 {
+  if (x > 12 || y > 12)
+    return x * y;
+
   return (x < 0 || y < 0 || x > 9 || y > 9) ? 1 : 0;
 }
 
@@ -63,9 +41,9 @@ int testSingleRay(Unit startX, Unit startY, Unit dirX, Unit dirY,
     h.position.y >= expectPointY - tolerateError;
 
   if (result)
-    printf("\nOK\n");
+    printf("\nOK\n\n");
   else
-    printf("\nFAIL\n");
+    printf("\nFAIL\n\n");
 
   return result;
 }
@@ -83,5 +61,14 @@ int main()
     16))
     return 1; 
 
+  if (!testSingleRay(
+    0,
+    0,
+    100, 100,
+    10, 9,
+    10240, 10239,
+    16))
+    return 1; 
+
   return 0;
 }