Fixes to substitution; using new C code for roughly 50% speedup