C Implementation of Algorithm