I'm gonna build a robot based on this algorithm. It's pretty slow on scratch and the algorithm is simple, but it should do the job.