Can we try to use function templates here?  IMO it whould really reduce the amount of duplicated code.