Improved matrix interface (now a template class) and fixed few minor issues