Finished C++ code. Ported to C. Fully working.