Added c implementation. Results seem off - more testing needed.