Current algorithm finished and working