Answer 1

Here is something I put together really quick. It is not pretty, but it works.

http://cpp.sh/2ib2k

I commented only the first part, but the rest of the algorithm follows the same logic.