Safety algorithm now works much more like intuitive expected
Sadly this also makes the whole algorithm even less understandable.