findSafePos Logic Rework