First commit of partial source. Some Matrix and vector handling