diff --git a/raycastlib.h b/raycastlib.h
index 160320c..11788a8 100644
--- a/raycastlib.h
+++ b/raycastlib.h
@@ -245,6 +245,7 @@ void castRaySquare(Ray localRay, Vector2D *nextCellOffset,
       collisionPointOffset->c2 = \
         (((int32_t) collisionPointOffset->c1) * localRay.direction.c2) /\
         ((localRay.direction.c1 == 0) ? 1 : localRay.direction.c1);\
+      Unit nextC2 = localRay.start.c2 + collisionPointOffset->c2;\
     }
 
   #define helper2(n1,n2,c)\
@@ -255,41 +256,44 @@ void castRaySquare(Ray localRay, Vector2D *nextCellOffset,
       
   if (localRay.direction.x > 0)
   {
-    criticalLine.start.x = UNITS_PER_SQUARE;
+    criticalLine.start.x = UNITS_PER_SQUARE - 1;
 
     if (localRay.direction.y > 0)
     {
       // top right
-      criticalLine.start.y = UNITS_PER_SQUARE;
+      criticalLine.start.y = UNITS_PER_SQUARE - 1;
       helper2(1,1,1)
     }
     else
     {
       // bottom right
-      criticalLine.start.y = -1;
+      criticalLine.start.y = 0;
       helper2(-1,1,0)
     }
   }
   else
   {
-    criticalLine.start.x = -1;
+    criticalLine.start.x = 0;
 
     if (localRay.direction.y > 0)
     {
       // top left
-      criticalLine.start.y = UNITS_PER_SQUARE;
+      criticalLine.start.y = UNITS_PER_SQUARE - 1;
       helper2(1,-1,0)
     }
     else
     {
       // bottom left
-      criticalLine.start.y = -1;
+      criticalLine.start.y = 0;
       helper2(-1,-1,1)
     }
   }
 
   #undef helper2
   #undef helper
+
+  collisionPointOffset->x += nextCellOffset->x;
+  collisionPointOffset->y += nextCellOffset->y;
 }
 
 void castRayMultiHit(Ray ray, int16_t (*arrayFunc)(int16_t, int16_t),
diff --git a/test.c b/test.c
index 5b3ffcb..167d268 100644
--- a/test.c
+++ b/test.c
@@ -6,6 +6,7 @@
 
 #include <stdio.h>
 #include "raycastlib.h"
+#include <time.h>
 
 int16_t testArrayFunc(int16_t x, int16_t y)
 {
@@ -72,10 +73,19 @@ int main()
     0,
     100, 100,
     10, 9,
-    10240, 10239,
+    10240, 10240,
     16))
     return 1; 
 
+  if (!testSingleRay(
+    400,
+    6811,
+    -629,805,
+    -1, 7,
+    -1, 7325,
+    16))
+    return 1;
+
   for (Unit i = -UNITS_PER_SQUARE; i <= UNITS_PER_SQUARE; i += 64)
   {
     Unit v = sinInt(i);
@@ -88,5 +98,20 @@ int main()
     //printf("\n");
   }
 
+  printf("benchmark:\n");
+
+time_t start,end;
+double dif;
+
+  time (&start);
+
+for (int i = 0; i < 1000; ++i)
+  printf("*");
+
+  time (&end);
+  dif = difftime (end,start);
+  printf ("Your calculations took %.2lf seconds to run.\n", dif );
+
+
   return 0;
 }