 		else if (isDeadEnd())
 		{
 			direction = backward;
-			if (isFirstRun)
-			{
-				path[pathLength] = backward;
-				simplifyMaze();
-				pathLength++;
-			}
-			else
-			{
-				pathPositionInLaterRun++;
-			}
+			storeTurnToPath();
 		}
 		break;
 

	}
}

void lightLed(unsigned char index)
{
	digitalWrite(ledPins[index], HIGH);
}

void turnOffAllLeds()

	{
		direction = path[pathPositionInLaterRun];
		pathPositionInLaterRun++;
	}
	else
	{
		// 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;
			}
			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()
