Synced with the C++ version

Just updating the C version with the same optimizations added to the C++ code.