Algorithm now seems to work fine. More tuning needed though.