 	{
 		direction = path[pathPositionInLaterRun];
 		pathPositionInLaterRun++;
-		return;
-	}
-
-	// Check if there is a way up front
-	for (unsigned char i = 1; i < sizeof(sensorPins) - 1; i++)
-	{
-		if (sensorValues[i] > threshold)
-		{
-			isEachDiversionOnCrossing[forward] = true;
-			break;
-		}
-	}
-
-	// check if the destination was reached
-	bool destinationWasReached = true;
-	for (int i = 0; i < sizeof(sensorPins); i++)
-	{
-		if (sensorValues[i] < threshold)
-		{
-			destinationWasReached = false;
-		}
-	}
-
-	// Stop if destination was reached
-	// Else go left preferably
-	if (destinationWasReached)
-	{
-		printPath();
-		direction = none;
 	}
 	else
 	{
-		if (isEachDiversionOnCrossing[left])
-		{
-			direction = left;
-		}
-		else if (isEachDiversionOnCrossing[forward])
-		{
-			direction = forward;
+		// Check if there is a way up front
+		for (unsigned char i = 1; i < sizeof(sensorPins) - 1; i++)
+		{
+			if (sensorValues[i] > threshold)
+			{
+				isEachDiversionOnCrossing[forward] = true;
+				break;
+			}
+		}
+
+		// check if the destination was reached
+		bool destinationWasReached = true;
+		for (int i = 0; i < sizeof(sensorPins); i++)
+		{
+			if (sensorValues[i] < threshold)
+			{
+				destinationWasReached = false;
+			}
+		}
+
+		// Stop if destination was reached
+		// Else go left preferably
+		if (destinationWasReached)
+		{
+			printPath();
+			direction = none;
 		}
 		else
 		{
-			direction = right;
-		}
-
-		path[pathLength] = direction;
-		pathLength++;
-		simplifyMaze();
-	}
-
-	// Reset for next crossing
-	for (unsigned char i = 0; i < sizeof(isEachDiversionOnCrossing); i++)
-	{
-		isEachDiversionOnCrossing[i] = false;
-	}
+			if (isEachDiversionOnCrossing[left])
+			{
+				direction = left;
+			}
+			else if (isEachDiversionOnCrossing[forward])
+			{
+				direction = forward;
+			}
+			else
+			{
+				direction = right;
+			}
+
+			storeTurnToPath();
+		}
+
+		// Reset for next crossing
+		for (unsigned char i = 0; i < sizeof(isEachDiversionOnCrossing); i++)
+		{
+			isEachDiversionOnCrossing[i] = false;
+		}
+	}
+}
+
+void storeTurnToPath()
+{
+	Serial.println(direction);
+	path[pathLength] = direction;
+	pathLength++;
+	//simplifyMaze();
 }
 
 void startFurtherDiversionCheckingTime()

	}

	// The path is now two steps shorter.
	pathLength -= 2;
}


		else if (isDeadEnd())
		{
			direction = backward;
			storeTurnToPath();
		}
		break;

