Working algorithm now