almost completed port to armadillo and more c++ like style