Fixed the C code. Needs memory management still.