finally working: compared with armadillo routine