Rcpp code working for regular indexing