The whole code changed to use vectors from "eigen"; much faster