C implementation added.

C implemetation added. Prevoius matlab implementation is now located in
the folder matlab.