Added Frew C Code

Converted matlab code into c code. Untested.