Adjusted algorithm - seems to be a bit more stable