C version of algorithm

This is the simple implementation of the algorithm in C.