Clean up
Clean up and rewrite to C
Fix misc errors