Updated, there may be segfaults, 

I can't compile, so I can't tell if it runs or not. But the basic idea is there if you follow the program.