Added a c implementation. Still needs debugging.