There are opensource ATU projects out there that already cover most of those requirements. Maybe its time to forget search algorithms altogether and move to using the like of the AD8302 gain phase detector and calculate directly.? Its on my bench but along with too many other projects.
Ian
VK2AMA