I get a headache looking at it 
Is this basically the same thing we worked on back in September?
But with a few tweaks added in? I had incorporated what we worked on back then into firmware and activated using it if you entered the chain sag value as a negative number. I can take your python and convert it to cpp if that’s what you need if you want to test it out (by entering a negative chain sag value). Honestly, there’s very little difference between the python code and the cpp code… just some brackets, semicolons, you don’t need to name the math routines.
Is chain elasticity and important factor at this point to test out (that might require a more intensive change to add another setting… it could be hard coded to something if you’d like).