Rewrote to use arrays instead of Armadillo vectors.