Hello,
expert in C programming here. I can implement the missing functionality from your attached code, profile it for speed and speed up the slower parts.
The code will be well commented and I will also be available for clarifications.
Thanks and let me know if you are interested to work with me,
Paul