All that debug printing will slow down the responsiveness. Record a set of times, then print them all out
when timing isn't critical.
I think given the inherent jitter you'll need a software PLL of some form to lock in to a stable frequency.
It could be as simple as recording a set of times and comparing current time to the Nth one back
in the set, and dividing by N for a better estimate of the period.