Ported python code to c
Ported python code to c, untested