Написать программу для подсчёта количества вхождений подстроки в строку. (язык с++)

#include <iostream>
#include <string>
using namespace std;
int main()
{
    string str, substr;
    cout << "String = "; getline(cin, str);
    cout << "Substring = "; cin >> substr;
    int cnt = 0;
    for(int i = 0; i < str.length(); ++i)
        if(str[i] == substr[0])
            if(str.substr(i, substr.length()) == substr)
            {
                ++cnt;
                i += substr.length() - 1;
            }
    cout << cnt;
    return 0;
}

//
Пример:
String = test testHelloWorld testtest1243test
Substring = test
5