[partial_tracking,sms] Fix bug in SMS partial tracking.

GetNextClosestPeak was missing peaks in some
situations.

Some general tidy up of SMS partial tracking code.