Made C implementation working properly