Just to let you know progress is being made. I've tracked down the "left-turn-bug" maybe better known as S-turn bug. headingErr and headingDiff are agnostic to left/right. This means the code detects there is a tightening curve, but actually it's a s-curve.
The CSV file being produced by the unit_tests could be used by a menu option in the future. This would help with post mortem debugging. I now have a visual debugger showing a path and all vectors. Pre-alpha but if there was to be a user menu option I'll release it.