I didn't even analyse the code that deeply, but indeed no proper time measurement done there.
40 kHz, digitalRead(), Serial.print() - that are things that simply don't go well together. No need to go further. Conclusion is simple: the code will have to be rewritten from scratch.