C++ substring searching function implemented