Printing debug values and fixed error when dividing by the diag vector