Very basic safe path algorithm