 	}
 
 	// The path is now two steps shorter.
+	path[pathLength - 1] = none;
+	path[pathLength - 2] = none;
 	pathLength -= 2;
 }
 

	{
		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;
		}
		else
		{
			direction = right;
		}

		path[pathLength] = direction;
		pathLength++;
		simplifyMaze();
	}

	// Reset for next crossing
	for (unsigned char i = 0; i < sizeof(isEachDiversionOnCrossing); i++)
	{
		isEachDiversionOnCrossing[i] = false;
	}
}

void startFurtherDiversionCheckingTime()

	}
}

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

void turnOffAllLeds()
