Writing this using Rcpp
Working on the C++ function