Fixed to run tests for C implementaton.
Refactored to make code more simple.