Working C version
Tweaks