Update diagonalize.cc