moved to Eigen matrices for speed, introduced some bugs