Got algorithm working