I have implemented the frame of the algorithm. Next step is to debug.