You're missing the modulus operation. It's kind of hidden in a single line of code.
Matlab version:
message_indices = mod((0:(length(decrypted_track)-1)) * 5, length(decrypted_track)) + 1;
Equivalent in C/C++/Java would be:
for (int i = 0; i < n; i++)
data[i] = orig_data[i * 5 % n];