Generally, pretty efficient implementations of such should be already provided in most core libraries along with the data structures in question.

Are you practicing for l33tcode or something?