implemented most of algorithm. Now debugging