Moved helper functions from anonymous namespace to private in header. Fixed bugs with insert function