New Algorithm first push

Change to new focus algorithm, not tested