Updated algorithm, fix epsilon on right check