Reformat, rearrange, tweak, and document

- Add comments to several functions
- Add an option to use Fast SQRT for Delta IK
- Group related functions together
- Outdent some leveling-related functions