First working implementation, at least with the one test used so far (test1.txt)