Add `highest_set_bit` function

Add the following function in `src/util.hpp`:

    template<typename N> inline N highest_set_bit(N n);

This function returns the 1-based bit index of the most significant set
bit in `n`.